feat(manipulation): add VAMP planner backend

dimos/manipulation/manipulation_module.py

@@ -27,24 +27,34 @@
 from enum import Enum
 import threading
 import time
-from typing import TYPE_CHECKING, Any, TypeAlias
+from typing import TYPE_CHECKING, Any, Self, TypeAlias
+import warnings
 
 import numpy as np
-from pydantic import Field
+from pydantic import Field, model_validator
 
 from dimos.agents.annotation import skill
 from dimos.agents.skill_result import SkillResult
 from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT
 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_kinematics, create_planner
+from dimos.manipulation.planning.factory import (
+    create_kinematics,
+    create_planner,
+    validate_planning_stack_config,
+)
 from dimos.manipulation.planning.kinematics.config import (
     JacobianKinematicsConfig,
     ManipulationKinematicsConfig,
     kinematics_config_from_name,
 )
 from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor
+from dimos.manipulation.planning.planners.config import (
+    MANIPULATION_PLANNER_CONFIG_ADAPTER,
+    ManipulationPlannerConfig,
+    RRTConnectPlannerConfig,
+)
 from dimos.manipulation.planning.spec.config import RobotModelConfig
 from dimos.manipulation.planning.spec.enums import IKStatus, ObstacleType
 from dimos.manipulation.planning.spec.models import (
@@ -58,8 +68,11 @@
 from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import (
     JointTrajectoryGenerator,
 )
+from dimos.manipulation.planning.vamp.errors import UnsupportedWorldCapabilityError
+from dimos.manipulation.planning.world.config import DrakeWorldConfig, ManipulationWorldConfig
 from dimos.manipulation.skill_errors import ManipulationSkillError
 from dimos.msgs.geometry_msgs.Pose import Pose
+from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.geometry_msgs.Quaternion import Quaternion
 from dimos.msgs.geometry_msgs.Vector3 import Vector3
 from dimos.msgs.sensor_msgs.JointState import JointState
@@ -101,7 +114,10 @@ class ManipulationModuleConfig(ModuleConfig):
     robots: list[RobotModelConfig] = Field(default_factory=list)
     planning_timeout: float = 10.0
     enable_viz: bool = False
-    planner_name: str = "rrt_connect"  # "rrt_connect"
+    world: ManipulationWorldConfig = Field(default_factory=DrakeWorldConfig)
+    planner: ManipulationPlannerConfig = Field(default_factory=RRTConnectPlannerConfig)
+    # Deprecated: use planner.backend instead.
+    planner_name: str | None = None
     kinematics: ManipulationKinematicsConfig = Field(default_factory=JacobianKinematicsConfig)
     # Deprecated: use kinematics.backend instead.
     kinematics_name: str | None = None  # "jacobian", "drake_optimization", or "pink"
@@ -110,6 +126,29 @@ class ManipulationModuleConfig(ModuleConfig):
     # Set to None to disable.
     floor_z: float | None = None
 
+    @model_validator(mode="after")
+    def apply_legacy_flat_backend_fields(self) -> Self:
+        """Apply deprecated flat backend fields as noisy compatibility shims."""
+        if self.planner_name is not None:
+            warnings.warn(
+                "ManipulationModuleConfig.planner_name is deprecated; use "
+                "planner={'backend': ...} instead.",
+                DeprecationWarning,
+                stacklevel=3,
+            )
+            self.planner = MANIPULATION_PLANNER_CONFIG_ADAPTER.validate_python(
+                {"backend": self.planner_name}
+            )
+        if self.kinematics_name is not None:
+            warnings.warn(
+                "ManipulationModuleConfig.kinematics_name is deprecated; use "
+                "kinematics={'backend': ...} instead.",
+                DeprecationWarning,
+                stacklevel=3,
+            )
+            self.kinematics = kinematics_config_from_name(self.kinematics_name)
+        return self
+
 
 class ManipulationModule(Module):
     """Base motion planning module with ControlCoordinator execution.
@@ -178,7 +217,16 @@ def _initialize_planning(self) -> None:
             logger.warning("No robots configured, planning disabled")
             return
 
-        self._world_monitor = WorldMonitor(enable_viz=self.config.enable_viz)
+        validate_planning_stack_config(
+            world=self.config.world,
+            planner=self.config.planner,
+            kinematics=self.config.kinematics,
+        )
+
+        self._world_monitor = WorldMonitor(
+            config=self.config.world,
+            enable_viz=self.config.enable_viz,
+        )
 
         for robot_config in self.config.robots:
             robot_id = self._world_monitor.add_robot(robot_config)
@@ -216,11 +264,8 @@ def _initialize_planning(self) -> None:
             if url := self._world_monitor.get_visualization_url():
                 logger.info(f"Visualization: {url}")
 
-        self._planner = create_planner(name=self.config.planner_name)
-        kinematics_config = self.config.kinematics
-        if self.config.kinematics_name is not None:
-            kinematics_config = kinematics_config_from_name(self.config.kinematics_name)
-        self._kinematics = create_kinematics(config=kinematics_config)
+        self._planner = create_planner(config=self.config.planner)
+        self._kinematics = create_kinematics(config=self.config.kinematics)
 
         # Start TF publishing thread if any robot has tf_extra_links
         if any(c.tf_extra_links for _, c, _ in self._robots.values()):
@@ -470,22 +515,22 @@ def _solve_ik_for_pose(
         """Run the configured kinematics backend for a world-frame pose."""
         assert self._world_monitor and self._kinematics
 
-        # Convert Pose to PoseStamped for the IK solver
-        from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
-
         target_pose = PoseStamped(
             frame_id="world",
             position=pose.position,
             orientation=pose.orientation,
         )
 
-        return self._kinematics.solve(
-            world=self._world_monitor.world,
-            robot_id=robot_id,
-            target_pose=target_pose,
-            seed=seed,
-            check_collision=check_collision,
-        )
+        try:
+            return self._kinematics.solve(
+                world=self._world_monitor.world,
+                robot_id=robot_id,
+                target_pose=target_pose,
+                seed=seed,
+                check_collision=check_collision,
+            )
+        except UnsupportedWorldCapabilityError as exc:
+            return IKResult(status=IKStatus.NO_SOLUTION, message=str(exc))
 
     @rpc
     def solve_ik(
@@ -548,7 +593,8 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool:
 
         ik = self._solve_ik_for_pose(robot_id, pose, current, check_collision=True)
         if not ik.is_success() or ik.joint_state is None:
-            return self._fail(f"IK failed: {ik.status.name}")
+            detail = f": {ik.message}" if ik.message else ""
+            return self._fail(f"IK failed: {ik.status.name}{detail}")
 
         logger.info(f"IK solved, error: {ik.position_error:.4f}m")
         return self._plan_path_only(robot_name, robot_id, ik.joint_state)

dimos/manipulation/planning/factory.py

@@ -25,23 +25,42 @@
     PinkKinematicsConfig,
     kinematics_config_from_name,
 )
+from dimos.manipulation.planning.planners.config import (
+    MANIPULATION_PLANNER_CONFIG_ADAPTER,
+    ManipulationPlannerConfig,
+    RRTConnectPlannerConfig,
+    VampPlannerConfig,
+)
+from dimos.manipulation.planning.world.config import (
+    MANIPULATION_WORLD_CONFIG_ADAPTER,
+    DrakeWorldConfig,
+    ManipulationWorldConfig,
+    VampWorldConfig,
+)
 
 if TYPE_CHECKING:
     from dimos.manipulation.planning.spec.protocols import KinematicsSpec, PlannerSpec, WorldSpec
 
 
 def create_world(
     backend: str = "drake",
+    config: ManipulationWorldConfig | None = None,
     enable_viz: bool = False,
     **kwargs: Any,
 ) -> WorldSpec:
-    """Create a world instance. backend='drake', enable_viz for Meshcat."""
-    if backend == "drake":
+    """Create a world instance from a backend name or typed world config."""
+    if config is None:
+        config = MANIPULATION_WORLD_CONFIG_ADAPTER.validate_python({"backend": backend})
+
+    if isinstance(config, DrakeWorldConfig):
         from dimos.manipulation.planning.world.drake_world import DrakeWorld
 
         return DrakeWorld(enable_viz=enable_viz, **kwargs)
-    else:
-        raise ValueError(f"Unknown backend: {backend}. Available: ['drake']")
+    if isinstance(config, VampWorldConfig):
+        from dimos.manipulation.planning.world.vamp_world import VampWorld
+
+        return VampWorld(config=config, **kwargs)
+    raise TypeError(f"Unsupported world config: {type(config).__name__}")
 
 
 def create_kinematics(
@@ -73,30 +92,72 @@ def create_kinematics(
 
 def create_planner(
     name: str = "rrt_connect",
+    config: ManipulationPlannerConfig | None = None,
     **kwargs: Any,
 ) -> PlannerSpec:
-    """Create motion planner. name='rrt_connect'."""
-    if name == "rrt_connect":
+    """Create motion planner from a backend name or typed planner config."""
+    if config is None:
+        config = MANIPULATION_PLANNER_CONFIG_ADAPTER.validate_python({"backend": name})
+
+    if isinstance(config, RRTConnectPlannerConfig):
         from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner
 
-        return RRTConnectPlanner(**kwargs)
-    else:
-        raise ValueError(f"Unknown planner: {name}. Available: ['rrt_connect']")
+        return RRTConnectPlanner(
+            step_size=config.step_size,
+            connect_step_size=config.connect_step_size,
+            goal_tolerance=config.goal_tolerance,
+            collision_step_size=config.collision_step_size,
+            **kwargs,
+        )
+    if isinstance(config, VampPlannerConfig):
+        from dimos.manipulation.planning.planners.vamp_planner import VampPlanner
+
+        return VampPlanner(config=config, **kwargs)
+    raise TypeError(f"Unsupported planner config: {type(config).__name__}")
+
+
+def validate_planning_stack_config(
+    world: ManipulationWorldConfig,
+    planner: ManipulationPlannerConfig,
+    kinematics: ManipulationKinematicsConfig,
+) -> None:
+    """Validate that selected world, planner, and kinematics backends can pair."""
+    if isinstance(planner, VampPlannerConfig) and not isinstance(world, VampWorldConfig):
+        raise ValueError("VAMP planner requires world backend 'vamp'")
+    if isinstance(world, VampWorldConfig) and not isinstance(planner, VampPlannerConfig):
+        raise ValueError("VAMP world backend requires planner backend 'vamp'")
+    if isinstance(kinematics, DrakeOptimizationKinematicsConfig) and not isinstance(
+        world, DrakeWorldConfig
+    ):
+        raise ValueError("Drake optimization kinematics requires world backend 'drake'")
 
 
 def create_planning_stack(
     robot_config: Any,
     enable_viz: bool = False,
+    world: ManipulationWorldConfig | None = None,
     planner_name: str = "rrt_connect",
+    planner: ManipulationPlannerConfig | None = None,
     kinematics_name: str = "jacobian",
     kinematics: ManipulationKinematicsConfig | None = None,
 ) -> tuple[WorldSpec, KinematicsSpec, PlannerSpec, str]:
     """Create complete planning stack. Returns (world, kinematics, planner, robot_id)."""
-    world = create_world(backend="drake", enable_viz=enable_viz)
-    kinematics_solver = create_kinematics(name=kinematics_name, config=kinematics)
-    planner = create_planner(name=planner_name)
-
-    robot_id = world.add_robot(robot_config)
-    world.finalize()
-
-    return world, kinematics_solver, planner, robot_id
+    world_config = world if world is not None else DrakeWorldConfig()
+    planner_config = (
+        planner
+        if planner is not None
+        else MANIPULATION_PLANNER_CONFIG_ADAPTER.validate_python({"backend": planner_name})
+    )
+    kinematics_config = (
+        kinematics if kinematics is not None else kinematics_config_from_name(kinematics_name)
+    )
+    validate_planning_stack_config(world_config, planner_config, kinematics_config)
+
+    world_backend = create_world(config=world_config, enable_viz=enable_viz)
+    kinematics_solver = create_kinematics(name=kinematics_name, config=kinematics_config)
+    planner_backend = create_planner(name=planner_name, config=planner_config)
+
+    robot_id = world_backend.add_robot(robot_config)
+    world_backend.finalize()
+
+    return world_backend, kinematics_solver, planner_backend, robot_id

dimos/manipulation/planning/monitor/world_monitor.py

@@ -25,6 +25,10 @@
 from dimos.manipulation.planning.monitor.robot_state_monitor import RobotStateMonitor
 from dimos.manipulation.planning.monitor.world_obstacle_monitor import WorldObstacleMonitor
 from dimos.manipulation.planning.spec.protocols import VisualizationSpec
+from dimos.manipulation.planning.world.config import (
+    MANIPULATION_WORLD_CONFIG_ADAPTER,
+    ManipulationWorldConfig,
+)
 from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped
 from dimos.msgs.sensor_msgs.JointState import JointState
 from dimos.utils.logging_config import setup_logger
@@ -55,11 +59,17 @@ class WorldMonitor:
     def __init__(
         self,
         backend: str = "drake",
+        config: ManipulationWorldConfig | None = None,
         enable_viz: bool = False,
         **kwargs: Any,
     ) -> None:
-        self._backend = backend
-        self._world: WorldSpec = create_world(backend=backend, enable_viz=enable_viz, **kwargs)
+        world_config = (
+            config
+            if config is not None
+            else MANIPULATION_WORLD_CONFIG_ADAPTER.validate_python({"backend": backend})
+        )
+        self._backend = world_config.backend
+        self._world: WorldSpec = create_world(config=world_config, enable_viz=enable_viz, **kwargs)
         self._visualization: VisualizationSpec | None = (
             self._world if isinstance(self._world, VisualizationSpec) else None
         )

dimos/manipulation/planning/planners/config.py

@@ -0,0 +1,60 @@
+# Copyright 2025-2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Configuration models for manipulation planner backends."""
+
+from __future__ import annotations
+
+from typing import Annotated, Literal
+
+from pydantic import Field, TypeAdapter
+
+from dimos.protocol.service.spec import BaseConfig
+
+
+class RRTConnectPlannerConfig(BaseConfig):
+    """Configuration for the backend-agnostic RRT-Connect planner."""
+
+    backend: Literal["rrt_connect"] = "rrt_connect"
+    step_size: float = 0.1
+    connect_step_size: float = 0.05
+    goal_tolerance: float = 0.1
+    collision_step_size: float = 0.02
+
+
+class VampPlannerConfig(BaseConfig):
+    """Configuration for the VAMP-native joint-space planner adapter."""
+
+    backend: Literal["vamp"] = "vamp"
+    algorithm: Literal["rrtc", "prm", "fcit", "aorrtc"] = "rrtc"
+    simplify: bool = True
+    validate_path: bool = True
+
+
+ManipulationPlannerConfig = Annotated[
+    RRTConnectPlannerConfig | VampPlannerConfig,
+    Field(discriminator="backend"),
+]
+
+MANIPULATION_PLANNER_CONFIG_ADAPTER: TypeAdapter[ManipulationPlannerConfig] = TypeAdapter(
+    ManipulationPlannerConfig
+)
+
+
+__all__ = [
+    "MANIPULATION_PLANNER_CONFIG_ADAPTER",
+    "ManipulationPlannerConfig",
+    "RRTConnectPlannerConfig",
+    "VampPlannerConfig",
+]