feat(manipulation): add trajectory parametrization

dimos/control/test_control.py

@@ -973,7 +973,7 @@ def test_tick_loop_calls_compute(self, mock_adapter):
 
 
 class TestIntegration:
-    def test_full_trajectory_execution(self, mock_adapter):
+    def test_full_trajectory_execution(self, mock_adapter, wait_until):
         component = HardwareComponent(
             hardware_id="arm",
             hardware_type=HardwareType.MANIPULATOR,
@@ -1017,10 +1017,15 @@ def test_full_trajectory_execution(self, mock_adapter):
         )
 
         tick_loop.start()
-        traj_task.execute(trajectory, trajectory_start_positions(trajectory))
-
-        time.sleep(0.6)
-        tick_loop.stop()
+        try:
+            traj_task.execute(trajectory, trajectory_start_positions(trajectory))
+            wait_until(
+                lambda: traj_task.get_state() == TrajectoryState.COMPLETED,
+                timeout=2.0,
+                interval=0.01,
+            )
+        finally:
+            tick_loop.stop()
 
         assert traj_task.get_state() == TrajectoryState.COMPLETED
         assert mock_adapter.write_joint_positions.call_count > 0

dimos/manipulation/pick_and_place_module.py

@@ -482,7 +482,7 @@ def pick(
         robot = self._get_robot(robot_name)
         if robot is None:
             return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found")
-        rname, _, config, _ = robot
+        rname, _, config = robot
         pre_grasp_offset = config.pre_grasp_offset
 
         # 1. Generate grasps (uses already-cached detections — call scan_objects first)
@@ -589,7 +589,7 @@ def _place_with_orientation(
         robot = self._get_robot(robot_name)
         if robot is None:
             return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found")
-        rname, _, config, _ = robot
+        rname, _, config = robot
         pre_place_offset = config.pre_grasp_offset
 
         # Reduce pre-place height for far targets

dimos/manipulation/planning/README.md

@@ -1,6 +1,7 @@
 # Manipulation Planning Stack
 
-Motion planning for robotic manipulators. Backend-agnostic design with Drake implementation.
+Motion planning for robotic manipulators. The stack separates geometric path
+planning from conversion to an executable timed trajectory.
 
 ## Quick Start
 
@@ -17,7 +18,7 @@ python -i -m dimos.manipulation.planning.examples.manipulation_client  # termina
 ```
 
 In the interactive client:
-```python
+```python skip
 commands()              # List available commands
 joints()                # Get current joint positions
 plan([0.1] * 7)         # Plan to target
@@ -58,7 +59,7 @@ execute()               # Execute via coordinator
 
 ## Using ManipulationModule
 
-```python
+```python skip
 from pathlib import Path
 from dimos.manipulation import ManipulationModule
 from dimos.manipulation.planning.spec import RobotModelConfig
@@ -79,13 +80,112 @@ module = ManipulationModule(
     enable_viz=True,
     world_backend="drake",                # RoboPlan is the default
     planner={"backend": "rrt_connect"},    # RoboPlan is the default
+    trajectory_parametrization={"backend": "simple_trapezoid"},
     kinematics={"backend": "drake_optimization"}, # Or "jacobian" / "pink"
 )
 module.start()
 module.plan_to_joints([0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7])
 module.execute()  # Sends to coordinator
 ```
 
+## Path-to-Trajectory Lifecycle
+
+A joint-space planner normally returns an untimed geometric path. Before DimOS
+accepts a `GeneratedPlan`, the one trajectory-parametrization backend selected
+at startup converts that path into a timed `JointTrajectory`. DimOS then
+validates joint ordering, dimensions, finite values, strictly increasing time,
+and start and goal preservation. Each backend is responsible for generating
+motion within the velocity and acceleration limits it receives. A failure
+leaves no executable plan cached.
+
+When `trajectory_parametrization` is omitted, the world selects the matching
+default: `RoboPlanWorld` uses `roboplan_toppra`, while `DrakeWorld` uses
+`simple_trapezoid`. An explicit backend always overrides that default.
+
+This boundary is exposed internally as `TrajectoryParametrizerSpec`, alongside
+`PlannerSpec` and `WorldSpec`. Its implementations own conversion, validation,
+and `GeneratedPlan` construction; `ManipulationModule` only supplies the world,
+selected planning groups, planning result, and next-plan speed.
+
+A planner may instead return a trajectory that already contains timestamps and
+velocities. That result is already on the trajectory side of the boundary, so
+DimOS skips parametrization, preserves its timing, and applies the same
+canonical structural validation. This is not fallback: a failure of the
+selected parametrizer never invokes another backend.
+
+Preview and execution both consume the accepted stored trajectory. Execution
+may project globally named joints into each robot's local order, but it does not
+regenerate or retime the trajectory.
+
+When Viser is enabled, its **Next plan speed** slider selects a runtime
+reduction from `0.05` to `1.0`. The value multiplies the configured velocity
+and acceleration scales for the next plan. It does not modify the currently
+accepted plan: move the slider, then press **Plan** again. Joint-space paths
+apply the value during trajectory parametrization; Viser Cartesian requests
+pass it to the native planner before that planner produces timestamps.
+
+## Trajectory Parametrization
+
+The compatibility backend retains the existing segmented trapezoidal behavior:
+
+```python skip
+ManipulationModuleConfig(
+    trajectory_parametrization={
+        "backend": "simple_trapezoid",
+        "velocity_scale": 1.0,
+        "acceleration_scale": 1.0,
+        "points_per_segment": 50,
+    },
+)
+```
+
+RoboPlan TOPP-RA produces continuous timing across a geometric path:
+
+```python skip
+ManipulationModuleConfig(
+    world_backend="roboplan",
+    trajectory_parametrization={
+        "backend": "roboplan_toppra",
+        "output_period": 0.01,
+        "velocity_scale": 0.8,
+        "acceleration_scale": 0.8,
+        "fitting_mode": "linear_blend",
+        "max_blend_deviation": 0.01,
+    },
+)
+```
+
+The selectable fitting modes are `hermite`, `cubic`, `adaptive`, and
+`linear_blend`. Adaptive fitting also exposes `max_adaptive_iterations` and
+`max_adaptive_step_size`. `linear_blend` exposes `max_blend_deviation`.
+
+`roboplan_toppra` can parametrize a geometric path from any planner, but only
+when `world_backend="roboplan"`: it reuses the finalized `RoboPlanWorld` model
+and planning groups. Selecting it with another world fails during startup.
+DimOS pins RoboPlan to `0.5.1` for this integration.
+
+For every selected movable joint, the RoboPlan URDF must provide a finite,
+positive velocity limit. DimOS uses an authored extended acceleration limit
+when present; otherwise it temporarily inserts a global `2.0 rad/s²` fallback
+while composing the RoboPlan model. Formal per-joint acceleration overrides
+will replace this fallback.
+
+```xml
+<limit
+  lower="-3.14"
+  upper="3.14"
+  effort="100"
+  velocity="2.0"
+  acceleration="4.0"
+/>
+```
+
+RoboPlan scene limits are authoritative for this backend. The current
+`RobotModelConfig.max_velocity`, `velocity_limits`, and `max_acceleration`
+fields are not substituted when a URDF limit is missing. Missing or invalid
+limits fail plan materialization with the affected joint named. Formal
+globally named per-joint overrides are future work.
+
 ## RobotModelConfig Fields
 
 | Field | Description |
@@ -133,6 +233,7 @@ accepted.
 | Backend | Description |
 |---------|-------------|
 | `DrakeWorld` | Drake physics with Meshcat visualization |
+| `RoboPlanWorld` | RoboPlan model, collision scene, native planner, and TOPP-RA support |
 
 ## Blueprints
 

dimos/manipulation/planning/factory.py

@@ -30,7 +30,15 @@
     ManipulationPlannerConfig,
     RoboPlanPlannerConfig,
 )
-from dimos.manipulation.planning.spec.protocols import PlannerSpec
+from dimos.manipulation.planning.spec.protocols import (
+    PlannerSpec,
+    TrajectoryParametrizerSpec,
+)
+from dimos.manipulation.planning.trajectory_generator.config import (
+    RoboPlanTOPPRAParametrizationConfig,
+    SimpleTrapezoidParametrizationConfig,
+    TrajectoryParametrizationConfig,
+)
 from dimos.manipulation.visualization.config import (
     ManipulationVisualizationConfig,
     NoManipulationVisualizationConfig,
@@ -51,6 +59,7 @@ class PlanningSpecs:
     world_monitor: WorldMonitor
     kinematics: KinematicsSpec
     planner: PlannerSpec
+    trajectory_parametrizer: TrajectoryParametrizerSpec
 
 
 WorldBackend: TypeAlias = Literal["drake", "roboplan"]
@@ -73,8 +82,13 @@ def validate_backend_combination(
     world_backend: str = "roboplan",
     planner_backend: str = "roboplan",
     kinematics_name: str = DEFAULT_KINEMATICS_NAME,
+    trajectory_parametrization_backend: str | None = None,
 ) -> None:
     """Validate manipulation backend choices before constructing the stack."""
+    if trajectory_parametrization_backend is None:
+        trajectory_parametrization_backend = (
+            "roboplan_toppra" if world_backend == "roboplan" else "simple_trapezoid"
+        )
     if world_backend not in SUPPORTED_WORLD_BACKENDS:
         raise ValueError(
             f"Unknown backend: {world_backend}. Available: {list(SUPPORTED_WORLD_BACKENDS)}"
@@ -87,11 +101,44 @@ def validate_backend_combination(
         raise ValueError(
             f"Unknown kinematics solver: {kinematics_name}. Available: {list(SUPPORTED_KINEMATICS)}"
         )
+    if trajectory_parametrization_backend not in ("simple_trapezoid", "roboplan_toppra"):
+        raise ValueError(
+            f"Unknown trajectory parametrization backend: {trajectory_parametrization_backend}"
+        )
 
     if planner_backend == "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"')
+    if trajectory_parametrization_backend == "roboplan_toppra" and world_backend != "roboplan":
+        raise ValueError(
+            'trajectory_parametrization.backend="roboplan_toppra" requires world_backend="roboplan"'
+        )
+
+
+def create_trajectory_parametrizer(
+    config: TrajectoryParametrizationConfig,
+    *,
+    world_backend: str,
+) -> TrajectoryParametrizerSpec:
+    """Construct the one startup-selected path parametrizer."""
+    if config.backend == "roboplan_toppra" and world_backend != "roboplan":
+        raise ValueError(
+            'trajectory_parametrization.backend="roboplan_toppra" requires world_backend="roboplan"'
+        )
+    if isinstance(config, SimpleTrapezoidParametrizationConfig):
+        from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import (
+            SimpleTrapezoidParametrizer,
+        )
+
+        return SimpleTrapezoidParametrizer(config)
+    if isinstance(config, RoboPlanTOPPRAParametrizationConfig):
+        from dimos.manipulation.planning.trajectory_generator.roboplan_toppra_parametrizer import (
+            RoboPlanTOPPRAParametrizer,
+        )
+
+        return RoboPlanTOPPRAParametrizer(config)
+    raise TypeError(f"Unsupported trajectory parametrization config: {type(config).__name__}")
 
 
 def create_world(
@@ -173,6 +220,7 @@ def create_planning_specs(
     planner: ManipulationPlannerConfig | None = None,
     kinematics_name: str | None = None,
     kinematics: ManipulationKinematicsConfig | None = None,
+    trajectory_parametrization: TrajectoryParametrizationConfig | None = None,
 ) -> PlanningSpecs:
     """Create planning specs around an already-created world."""
     from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor
@@ -183,17 +231,28 @@ def create_planning_specs(
         kinematics = kinematics_config_from_name(DEFAULT_KINEMATICS_NAME)
     if planner is None:
         planner = RoboPlanPlannerConfig()
+    if trajectory_parametrization is None:
+        trajectory_parametrization = (
+            RoboPlanTOPPRAParametrizationConfig()
+            if world_backend == "roboplan"
+            else SimpleTrapezoidParametrizationConfig()
+        )
 
     validate_backend_combination(
         world_backend=world_backend,
         planner_backend=planner.backend,
         kinematics_name=kinematics.backend,
+        trajectory_parametrization_backend=trajectory_parametrization.backend,
     )
 
     return PlanningSpecs(
         world_monitor=WorldMonitor(world=world),
         kinematics=create_kinematics(config=kinematics),
         planner=create_planner(config=planner, world=world, world_backend=world_backend),
+        trajectory_parametrizer=create_trajectory_parametrizer(
+            trajectory_parametrization,
+            world_backend=world_backend,
+        ),
     )
 
 

dimos/manipulation/planning/monitor/test_world_monitor.py

@@ -347,24 +347,27 @@ def test_obstacle_monitor_routes_mutations_through_parent_world_monitor(
     remove_obstacle.assert_called_once_with("parent-id")
 
 
-def test_create_planning_specs_wraps_existing_world(monkeypatch) -> None:
+def test_create_planning_specs_wraps_existing_world(mocker: MockerFixture) -> None:
     fake_world = FakeWorld()
     fake_kinematics = object()
     fake_planner = object()
+    fake_parametrizer = object()
 
-    monkeypatch.setattr(
+    mocker.patch.object(planning_factory, "create_kinematics", return_value=fake_kinematics)
+    mocker.patch.object(planning_factory, "create_planner", return_value=fake_planner)
+    mocker.patch.object(
         planning_factory,
-        "create_kinematics",
-        lambda *args, **kwargs: fake_kinematics,
+        "create_trajectory_parametrizer",
+        return_value=fake_parametrizer,
     )
-    monkeypatch.setattr(planning_factory, "create_planner", lambda **kwargs: fake_planner)
 
     planning_specs = planning_factory.create_planning_specs(world=fake_world)  # type: ignore[arg-type]
 
     assert planning_specs.world_monitor.world is fake_world
     assert planning_specs.world_monitor.visualization is None
     assert planning_specs.kinematics is fake_kinematics
     assert planning_specs.planner is fake_planner
+    assert planning_specs.trajectory_parametrizer is fake_parametrizer
 
 
 def test_world_monitor_exposes_planning_groups_and_duplicate_names_do_not_mutate() -> None: