diff --git a/CONTEXT.md b/CONTEXT.md deleted file mode 100644 index d97571b990..0000000000 --- a/CONTEXT.md +++ /dev/null @@ -1,27 +0,0 @@ -# Manipulation Planning - -This context describes requests for planning robot motion through joint and Cartesian spaces. - -## Language - -**Cartesian Waypoint**: -One absolute TCP pose or relative rigid displacement within a Cartesian target. - -**Cartesian Target**: -An ordered, homogeneous sequence of Cartesian waypoints for one planning group, including its starting waypoint. An absolute target contains only `PoseStamped` waypoints and starts at the current TCP pose. A relative target contains only `Transform` waypoints, starts with the identity transform, and measures every waypoint from the planning-start TCP pose. -_Avoid_: Cartesian track - -**Cartesian Path Configuration**: -Per-planning-call policy that selects how Cartesian waypoints are connected and constrains that operation. It is independent of the startup configuration that selects and constructs a planner backend. - -**Standard Cartesian Planning**: -Cartesian waypoint planning through a backend's supported serializable options. For RoboPlan, this includes multi-waypoint and simultaneous multi-end-effector paths, bounded and time-optimal speed modes, tracking tolerances, and solver tuning. - -**Bounded Speed Mode**: -A Cartesian timing policy that treats configured tool speeds and accelerations as maxima and slows the motion further when required by tracking or joint limits. - -**Time-Optimal Speed Mode**: -A Cartesian timing policy that resolves the requested path into joint space and retimes it against joint limits, optionally blending intermediate corners. - -**Custom Planner Components**: -Backend-native solver tasks, constraints, and barriers injected as live objects. These are outside standard Cartesian planning and require a separate constrained-IK interface. diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index c481965070..38853afe3a 100644 --- a/dimos/control/test_control.py +++ b/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 diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 137d8af72c..e4abcc8f63 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -80,9 +80,13 @@ RobotName, WorldRobotID, ) -from dimos.manipulation.planning.spec.protocols import KinematicsSpec, PlannerSpec -from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( - JointTrajectoryGenerator, +from dimos.manipulation.planning.spec.protocols import ( + KinematicsSpec, + PlannerSpec, + TrajectoryParametrizerSpec, +) +from dimos.manipulation.planning.trajectory_generator.config import ( + TrajectoryParametrizationConfig, ) from dimos.manipulation.skill_errors import ManipulationSkillError from dimos.manipulation.visualization.config import ( @@ -98,15 +102,13 @@ from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.tf2_msgs.TFMessage import TFMessage -from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory -from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint from dimos.utils.logging_config import setup_logger logger = setup_logger() # Composite type aliases for readability (using semantic IDs from planning.spec) -RobotEntry: TypeAlias = tuple[WorldRobotID, RobotModelConfig, JointTrajectoryGenerator] -"""(world_robot_id, config, trajectory_generator)""" +RobotEntry: TypeAlias = tuple[WorldRobotID, RobotModelConfig] +"""(world_robot_id, config)""" RobotRegistry: TypeAlias = dict[RobotName, RobotEntry] """Maps robot_name -> RobotEntry""" @@ -146,6 +148,13 @@ class ManipulationModuleConfig(ModuleConfig): default_factory=NoManipulationVisualizationConfig ) planner: ManipulationPlannerConfig = Field(default_factory=RoboPlanPlannerConfig) + trajectory_parametrization: TrajectoryParametrizationConfig | None = Field( + default=None, + description=( + "Path parametrizer selected at startup. Omit to use roboplan_toppra " + "with RoboPlanWorld or simple_trapezoid with DrakeWorld." + ), + ) kinematics: ManipulationKinematicsConfig = Field(default_factory=PinkKinematicsConfig) # Deprecated: use kinematics.backend instead. kinematics_name: KinematicsName | None = None @@ -179,13 +188,15 @@ def __init__(self, **kwargs: Any) -> None: self._lock = threading.Lock() self._error_message = "" self._planning_epoch = 0 + self._motion_speed_scale = 1.0 # Planning components (initialized in start()) self._world_monitor: WorldMonitor | None = None self._planner: PlannerSpec | None = None self._kinematics: KinematicsSpec | None = None + self._trajectory_parametrizer: TrajectoryParametrizerSpec | None = None - # Robot registry: maps robot_name -> (world_robot_id, config, trajectory_gen) + # Robot registry: maps robot_name -> (world_robot_id, config) self._robots: RobotRegistry = {} # Canonical generated plan for plan/preview/execute workflow. @@ -236,10 +247,12 @@ def _initialize_planning(self) -> None: planner=self.config.planner, kinematics_name=self.config.kinematics_name, kinematics=self.config.kinematics, + trajectory_parametrization=self.config.trajectory_parametrization, ) self._world_monitor = planning_specs.world_monitor self._planner = planning_specs.planner self._kinematics = planning_specs.kinematics + self._trajectory_parametrizer = planning_specs.trajectory_parametrizer visualization = create_manipulation_visualization( self.config.visualization, world=world, @@ -249,12 +262,7 @@ def _initialize_planning(self) -> None: for robot_config in self.config.robots: robot_id = self._world_monitor.add_robot(robot_config) - traj_gen = JointTrajectoryGenerator( - num_joints=len(robot_config.joint_names), - max_velocity=robot_config.max_velocity, - max_acceleration=robot_config.max_acceleration, - ) - self._robots[robot_config.name] = (robot_id, robot_config, traj_gen) + self._robots[robot_config.name] = (robot_id, robot_config) operator = ManipulationOperator(self, self._world_monitor) self._world_monitor.finalize(visualization, operator=operator) @@ -276,7 +284,7 @@ def _initialize_planning(self) -> None: self._world_monitor.add_obstacle(floor_obs) logger.info(f"Floor obstacle added at z={fz:.3f}") - for _, (robot_id, _, _) in self._robots.items(): + for _, (robot_id, _) in self._robots.items(): self._world_monitor.start_state_monitor(robot_id) if self._world_monitor.visualization is not None: @@ -285,7 +293,7 @@ def _initialize_planning(self) -> None: logger.info(f"Visualization: {url}") # Start TF publishing thread if any robot has tf_extra_links - if any(c.tf_extra_links for _, c, _ in self._robots.values()): + if any(c.tf_extra_links for _, c in self._robots.values()): self._tf_stop_event.clear() self._tf_thread = threading.Thread( target=self._tf_publish_loop, name="ManipTFThread", daemon=True @@ -301,14 +309,14 @@ def _get_default_robot_name(self) -> RobotName | None: def _get_robot( self, robot_name: RobotName | None = None - ) -> tuple[RobotName, WorldRobotID, RobotModelConfig, JointTrajectoryGenerator] | None: + ) -> tuple[RobotName, WorldRobotID, RobotModelConfig] | None: """Get robot by name or default. Args: robot_name: Robot name or None for default (if single robot) Returns: - (robot_name, robot_id, config, traj_gen) or None if not found + (robot_name, robot_id, config) or None if not found """ if not robot_name: # None or empty string (LLMs often pass "") robot_name = self._get_default_robot_name() @@ -320,8 +328,8 @@ def _get_robot( logger.error(f"Unknown robot: {robot_name}") return None - robot_id, config, traj_gen = self._robots[robot_name] - return (robot_name, robot_id, config, traj_gen) + robot_id, config = self._robots[robot_name] + return (robot_name, robot_id, config) def _on_joint_state(self, msg: JointState) -> None: """Callback when joint state received from driver. @@ -336,7 +344,7 @@ def _on_joint_state(self, msg: JointState) -> None: # Build name → index map once for the whole message name_to_idx = {name: i for i, name in enumerate(msg.name)} - for robot_name, (robot_id, config, _) in self._robots.items(): + for robot_name, (robot_id, config) in self._robots.items(): coord_names = config.get_coordinator_joint_names() indices = [name_to_idx.get(cn) for cn in coord_names] if any(idx is None for idx in indices): @@ -386,7 +394,7 @@ def _tf_publish_loop(self) -> None: if self._world_monitor is None: break transforms: list[Transform] = [] - for robot_id, config, _ in self._robots.values(): + for robot_id, config in self._robots.values(): # Publish world → EE ee_pose = self._world_monitor.get_ee_pose(robot_id) if ee_pose is not None: @@ -423,6 +431,27 @@ def get_error(self) -> str: """ return self._error_message + @rpc + def set_motion_speed(self, speed_scale: float) -> bool: + """Set a runtime speed reduction for plans generated in the future. + + Existing accepted plans and dispatched trajectories remain unchanged. + Plan again after changing this value. + """ + if not math.isfinite(speed_scale) or speed_scale <= 0.0 or speed_scale > 1.0: + self._record_error("motion speed scale must be finite, > 0, and <= 1") + return False + with self._lock: + self._motion_speed_scale = float(speed_scale) + self._error_message = "" + return True + + @rpc + def get_motion_speed(self) -> float: + """Return the runtime speed reduction used for future plans.""" + with self._lock: + return self._motion_speed_scale + @rpc def cancel(self) -> bool: """Cancel current motion or invalidate an in-progress plan.""" @@ -513,7 +542,7 @@ def is_collision_free(self, joints: list[float], robot_name: RobotName | None = robot_name: Robot to check (required if multiple robots configured) """ if (robot := self._get_robot(robot_name)) and self._world_monitor: - _, robot_id, config, _ = robot + _, robot_id, config = robot joint_state = JointState(name=config.joint_names, position=joints) return self._world_monitor.is_state_valid(robot_id, joint_state) return False @@ -567,149 +596,6 @@ def _require_unique_pose_group_id_for_robot(self, robot_name: RobotName) -> Plan ) return group_id - @staticmethod - def _assert_finite_sequence(values: Sequence[float], label: str) -> None: - for value in values: - if not math.isfinite(value): - raise ValueError(f"{label} contains non-finite value") - - def _limits_for_global_joints( - self, joint_names: Sequence[str] - ) -> tuple[list[float], list[float]]: - velocities: list[float] = [] - accelerations: list[float] = [] - for global_name in joint_names: - if "/" not in global_name: - raise ValueError(f"Joint '{global_name}' is not globally named") - robot_name, local_name = global_name.split("/", 1) - robot = self._get_robot(robot_name) - if robot is None: - raise ValueError(f"Unknown robot for joint '{global_name}'") - _, _, config, _ = robot - if local_name not in config.joint_names: - raise ValueError(f"Unknown local joint '{global_name}'") - velocity = float(config.max_velocity) - acceleration = float(config.max_acceleration) - if not math.isfinite(velocity) or velocity <= 0.0: - raise ValueError(f"Invalid velocity limit for '{global_name}'") - if not math.isfinite(acceleration) or acceleration <= 0.0: - raise ValueError(f"Invalid acceleration limit for '{global_name}'") - velocities.append(velocity) - accelerations.append(acceleration) - return velocities, accelerations - - def _validate_selected_path( - self, path: Sequence[JointState], expected_names: Sequence[str] - ) -> list[list[float]]: - if len(path) < 2: - raise ValueError("Planner returned fewer than two waypoints") - expected = list(expected_names) - waypoints: list[list[float]] = [] - for waypoint_index, state in enumerate(path): - if list(state.name) != expected: - raise ValueError( - f"Waypoint {waypoint_index} joint names do not match selected order" - ) - positions = list(state.position) - if len(positions) != len(expected): - raise ValueError(f"Waypoint {waypoint_index} position dimension mismatch") - self._assert_finite_sequence(positions, f"Waypoint {waypoint_index} positions") - waypoints.append(positions) - return waypoints - - def _validate_generated_trajectory( - self, - trajectory: JointTrajectory, - expected_names: Sequence[str], - waypoints: Sequence[Sequence[float]], - ) -> None: - expected = list(expected_names) - if list(trajectory.joint_names) != expected: - raise ValueError("Generated trajectory joint names do not match selected order") - if not trajectory.points: - raise ValueError("Generated trajectory has no points") - previous_time: float | None = None - for point_index, point in enumerate(trajectory.points): - if len(point.positions) != len(expected) or len(point.velocities) != len(expected): - raise ValueError(f"Generated point {point_index} dimension mismatch") - self._assert_finite_sequence( - point.positions, f"Generated point {point_index} positions" - ) - self._assert_finite_sequence( - point.velocities, f"Generated point {point_index} velocities" - ) - if not math.isfinite(point.time_from_start): - raise ValueError(f"Generated point {point_index} time is non-finite") - if point_index == 0 and point.time_from_start != 0.0: - raise ValueError("Generated trajectory must start at time 0") - if previous_time is not None and point.time_from_start <= previous_time: - raise ValueError("Generated trajectory times must be strictly increasing") - previous_time = point.time_from_start - non_noop = any(list(waypoint) != list(waypoints[0]) for waypoint in waypoints[1:]) - if non_noop and trajectory.duration <= 0.0: - raise ValueError("Generated trajectory duration must be positive") - waypoint_index = 0 - for point in trajectory.points: - if list(point.positions) == list(waypoints[waypoint_index]): - waypoint_index += 1 - if waypoint_index == len(waypoints): - break - if waypoint_index != len(waypoints): - raise ValueError("Generated trajectory does not contain ordered waypoint boundaries") - - def _materialize_generated_plan( - self, group_ids: tuple[PlanningGroupID, ...], result_path: Sequence[JointState] - ) -> tuple[list[JointState], JointTrajectory]: - assert self._world_monitor is not None - selection = self._world_monitor.planning_groups.select(group_ids) - expected_names = list(selection.joint_names) - path = [JointState(state) for state in result_path] - waypoints = self._validate_selected_path(path, expected_names) - velocities, accelerations = self._limits_for_global_joints(expected_names) - generator = JointTrajectoryGenerator( - num_joints=len(expected_names), - max_velocity=velocities, - max_acceleration=accelerations, - ) - generated = generator.generate(waypoints) - trajectory = JointTrajectory( - joint_names=expected_names, - points=generated.points, - timestamp=generated.timestamp, - ) - self._validate_generated_trajectory(trajectory, expected_names, waypoints) - return path, trajectory - - def _materialize_timed_generated_plan( - self, - group_ids: tuple[PlanningGroupID, ...], - result: PlanningResult, - ) -> tuple[list[JointState], JointTrajectory]: - """Preserve a planner-supplied timed trajectory without reparameterizing it.""" - assert self._world_monitor is not None - selection = self._world_monitor.planning_groups.select(group_ids) - expected_names = list(selection.joint_names) - path = [JointState(state) for state in result.path] - waypoints = self._validate_selected_path(path, expected_names) - timestamps = result.timestamps - if timestamps is None or len(timestamps) != len(path): - raise ValueError("Planner must return one timestamp per waypoint") - points: list[TrajectoryPoint] = [] - for waypoint_index, (state, timestamp) in enumerate(zip(path, timestamps, strict=True)): - velocities = list(state.velocity) - if len(velocities) != len(expected_names): - raise ValueError(f"Waypoint {waypoint_index} velocity dimension mismatch") - points.append( - TrajectoryPoint( - time_from_start=float(timestamp), - positions=list(state.position), - velocities=velocities, - ) - ) - trajectory = JointTrajectory(joint_names=expected_names, points=points) - self._validate_generated_trajectory(trajectory, expected_names, waypoints) - return path, trajectory - def _resolve_group_plan_start( self, group_ids: tuple[PlanningGroupID, ...], @@ -731,28 +617,21 @@ def _store_generated_plan( group_ids: tuple[PlanningGroupID, ...], result: PlanningResult, planning_epoch: int, - *, - preserve_timing: bool = False, ) -> GeneratedPlan | None: """Validate, materialize, and atomically store a successful planning result.""" try: - if preserve_timing: - path, trajectory = self._materialize_timed_generated_plan(group_ids, result) - else: - path, trajectory = self._materialize_generated_plan(group_ids, result.path) + if self._world_monitor is None or self._trajectory_parametrizer is None: + raise ValueError("Trajectory parametrizer is not initialized") + selection = self._world_monitor.planning_groups.select(group_ids) + plan = self._trajectory_parametrizer.materialize_plan( + world=self._world_monitor.world, + selection=selection, + result=result, + speed_scale=self.get_motion_speed(), + ) except Exception as exc: self._fail_planning_epoch(planning_epoch, f"Failed to materialize plan: {exc}") return None - plan = GeneratedPlan( - group_ids=group_ids, - trajectory=trajectory, - path=path, - status=result.status, - planning_time=result.planning_time, - path_length=result.path_length, - iterations=result.iterations, - message=result.message, - ) with self._lock: if self._state != ManipulationState.PLANNING or planning_epoch != self._planning_epoch: logger.info("Discarding cancelled planning result") @@ -914,7 +793,7 @@ def inverse_kinematics_single( robot = self._get_robot(robot_name) if robot is None: return IKResult(status=IKStatus.NO_SOLUTION, message="Robot not found") - selected_robot_name, _, _, _ = robot + selected_robot_name, _, _ = robot try: group_id = self._require_unique_pose_group_id_for_robot(selected_robot_name) except ValueError as exc: @@ -989,7 +868,7 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: if robot is None: self._record_error("Robot not found or robot_name is required") return False - selected_robot_name, _, _, _ = robot + selected_robot_name, _, _ = robot try: group_id = self._require_unique_pose_group_id_for_robot(selected_robot_name) except ValueError as exc: @@ -1018,7 +897,7 @@ def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None robot = self._get_robot(robot_name) if robot is None: return False - selected_robot_name, _, _, _ = robot + selected_robot_name, _, _ = robot logger.info( f"Planning to joints for {selected_robot_name}: {[f'{j:.3f}' for j in joints.position]}" ) @@ -1173,12 +1052,7 @@ def generate_cartesian_plan( planning_epoch, f"Cartesian planning failed: {result.status.name}{detail}" ) return None - return self._store_generated_plan( - group_ids, - result, - planning_epoch, - preserve_timing=True, - ) + return self._store_generated_plan(group_ids, result, planning_epoch) @rpc def preview_path( @@ -1302,7 +1176,7 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> RobotInfoPayloa if robot is None: return None - robot_name, robot_id, config, _ = robot + robot_name, robot_id, config = robot planning_groups = ( list(self._world_monitor.planning_groups.groups_for_robot(robot_name)) if self._world_monitor is not None @@ -1332,7 +1206,7 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> RobotInfoPayloa def robot_items(self) -> list[tuple[RobotName, WorldRobotID, RobotModelConfig]]: """Return configured robots for in-process visualization adapters.""" - return [(name, robot_id, config) for name, (robot_id, config, _) in self._robots.items()] + return [(name, robot_id, config) for name, (robot_id, config) in self._robots.items()] def robot_id_for_name(self, robot_name: RobotName) -> WorldRobotID | None: """Return the planning-world robot id for a configured robot name.""" @@ -1341,7 +1215,7 @@ def robot_id_for_name(self, robot_name: RobotName) -> WorldRobotID | None: def robot_name_for_id(self, robot_id: WorldRobotID) -> RobotName | None: """Return the configured robot name for a planning-world robot id.""" - for robot_name, (candidate_id, _, _) in self._robots.items(): + for robot_name, (candidate_id, _) in self._robots.items(): if candidate_id == robot_id: return robot_name return None @@ -1468,7 +1342,7 @@ def set_init_joints_to_current(self, robot_name: RobotName | None = None) -> boo robot = self._get_robot(robot_name) if robot is None: return False - robot_name_resolved, robot_id, _, _ = robot + robot_name_resolved, robot_id, _ = robot if self._world_monitor is None: return False current = self._world_monitor.get_current_joint_state(robot_id) @@ -1490,7 +1364,7 @@ def _initialize_execution(self) -> None: model_joint_names=config.joint_names, coordinator_to_model=config.joint_name_mapping, ) - for _, config, _ in self._robots.values() + for _, config in self._robots.values() ] self._execution_manager = PlanExecutionManager( targets=targets, @@ -1636,7 +1510,7 @@ def _get_gripper_hardware_id(self, robot_name: RobotName | None = None) -> str | robot = self._get_robot(robot_name) if robot is None: return None - _, _, config, _ = robot + _, _, config = robot if not config.gripper_hardware_id: logger.warning(f"No gripper_hardware_id configured for '{config.name}'") return None @@ -1874,7 +1748,7 @@ def move_to_joints( 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 goal = JointState(name=config.joint_names, position=joint_values) logger.info(f"Planning motion to joints [{', '.join(f'{j:.3f}' for j in joint_values)}]...") @@ -1902,7 +1776,7 @@ def go_home(self, robot_name: str | None = None) -> SkillResult[ManipulationSkil 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 if config.home_joints is None: return SkillResult.fail( @@ -1938,7 +1812,7 @@ def go_init(self, robot_name: str | None = None) -> SkillResult[ManipulationSkil robot = self._get_robot(robot_name) if robot is None: return SkillResult.fail("ROBOT_NOT_FOUND", "Robot not found") - rname, robot_id, _, _ = robot + rname, robot_id, _ = robot init = self._init_joints.get(rname) if init is None: diff --git a/dimos/manipulation/pick_and_place_module.py b/dimos/manipulation/pick_and_place_module.py index bc59adce37..b37f6e9882 100644 --- a/dimos/manipulation/pick_and_place_module.py +++ b/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 diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index a6bcb64c43..05288bda3e 100644 --- a/dimos/manipulation/planning/README.md +++ b/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,6 +80,7 @@ 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() @@ -86,6 +88,104 @@ 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 + +``` + +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 diff --git a/dimos/manipulation/planning/factory.py b/dimos/manipulation/planning/factory.py index f3e25dd5f0..b738f5bd33 100644 --- a/dimos/manipulation/planning/factory.py +++ b/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, + ), ) diff --git a/dimos/manipulation/planning/monitor/test_world_monitor.py b/dimos/manipulation/planning/monitor/test_world_monitor.py index b14000496d..cb19f79389 100644 --- a/dimos/manipulation/planning/monitor/test_world_monitor.py +++ b/dimos/manipulation/planning/monitor/test_world_monitor.py @@ -347,17 +347,19 @@ 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] @@ -365,6 +367,7 @@ def test_create_planning_specs_wraps_existing_world(monkeypatch) -> None: 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: diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index ff33953025..9214879155 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -34,6 +34,7 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( CartesianTarget, + GeneratedPlan, IKResult, Obstacle, PlanningGroupID, @@ -332,3 +333,18 @@ def plan_cartesian_path( def get_name(self) -> str: """Get planner name.""" ... + + +@runtime_checkable +class TrajectoryParametrizerSpec(Protocol): + """Convert successful planning output into one canonical generated plan.""" + + def materialize_plan( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + result: PlanningResult, + speed_scale: float = 1.0, + ) -> GeneratedPlan: + """Preserve timed output or parametrize an untimed path, then validate it.""" + ... diff --git a/dimos/manipulation/planning/trajectory_generator/config.py b/dimos/manipulation/planning/trajectory_generator/config.py new file mode 100644 index 0000000000..78e1d189a7 --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/config.py @@ -0,0 +1,49 @@ +# Copyright 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. + +"""Typed configuration for manipulation trajectory parametrization.""" + +from typing import Annotated, Literal + +from pydantic import Field + +from dimos.protocol.service.spec import BaseConfig + + +class SimpleTrapezoidParametrizationConfig(BaseConfig): + """Configuration for the compatibility segmented-trapezoid backend.""" + + backend: Literal["simple_trapezoid"] = "simple_trapezoid" + velocity_scale: float = Field(default=1.0, gt=0.0, le=1.0) + acceleration_scale: float = Field(default=1.0, gt=0.0, le=1.0) + points_per_segment: int = Field(default=50, ge=1) + + +class RoboPlanTOPPRAParametrizationConfig(BaseConfig): + """Configuration for RoboPlan TOPP-RA path parametrization.""" + + backend: Literal["roboplan_toppra"] = "roboplan_toppra" + output_period: float = Field(default=0.01, gt=0.0) + velocity_scale: float = Field(default=1.0, gt=0.0, le=1.0) + acceleration_scale: float = Field(default=1.0, gt=0.0, le=1.0) + fitting_mode: Literal["hermite", "cubic", "adaptive", "linear_blend"] = "linear_blend" + max_adaptive_iterations: int = Field(default=10, ge=1) + max_adaptive_step_size: float = Field(default=0.05, gt=0.0) + max_blend_deviation: float = Field(default=0.01, ge=0.0) + + +TrajectoryParametrizationConfig = Annotated[ + SimpleTrapezoidParametrizationConfig | RoboPlanTOPPRAParametrizationConfig, + Field(discriminator="backend"), +] diff --git a/dimos/manipulation/planning/trajectory_generator/parametrizer.py b/dimos/manipulation/planning/trajectory_generator/parametrizer.py new file mode 100644 index 0000000000..3497b29e94 --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/parametrizer.py @@ -0,0 +1,209 @@ +# Copyright 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. + +"""Shared implementation for the trajectory-parametrizer planning Spec.""" + +from abc import ABC, abstractmethod +from collections.abc import Sequence +import math + +from dimos.manipulation.planning.groups.models import PlanningGroupSelection +from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningResult +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint + +_TRAJECTORY_POSITION_TOLERANCE = 1e-6 + + +class TrajectoryParametrizationError(ValueError): + """Planning output could not be converted into a valid generated plan.""" + + +class BaseTrajectoryParametrizer(ABC): + """Own common PlanningResult-to-GeneratedPlan materialization.""" + + def materialize_plan( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + result: PlanningResult, + speed_scale: float = 1.0, + ) -> GeneratedPlan: + """Preserve timed output or parametrize an untimed path, then validate it.""" + self._validate_speed_scale(speed_scale) + if not result.is_success(): + raise TrajectoryParametrizationError( + f"Cannot materialize unsuccessful planning result: {result.status.name}" + ) + + path = [JointState(state) for state in result.path] + self._validate_selected_path(path, selection.joint_names) + if result.timestamps is None: + trajectory = self._parametrize_path(world, selection, tuple(path), speed_scale) + else: + trajectory = self._timed_trajectory(selection, path, result.timestamps) + self._validate_trajectory( + trajectory, + selection.joint_names, + expected_start=path[0].position, + expected_goal=path[-1].position, + ) + + return GeneratedPlan( + group_ids=selection.group_ids, + trajectory=trajectory, + path=path, + status=result.status, + planning_time=result.planning_time, + path_length=result.path_length, + iterations=result.iterations, + message=result.message, + ) + + @abstractmethod + def _parametrize_path( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + path: tuple[JointState, ...], + speed_scale: float, + ) -> JointTrajectory: + """Convert one validated untimed path using the selected backend.""" + + @staticmethod + def _validate_speed_scale(speed_scale: float) -> None: + if not math.isfinite(speed_scale) or speed_scale <= 0.0 or speed_scale > 1.0: + raise TrajectoryParametrizationError("speed_scale must be finite, > 0, and <= 1") + + @staticmethod + def _assert_finite_sequence(values: Sequence[float], label: str) -> None: + for value in values: + if not math.isfinite(value): + raise TrajectoryParametrizationError(f"{label} contains non-finite value") + + @classmethod + def _validate_selected_path( + cls, + path: Sequence[JointState], + expected_names: Sequence[str], + ) -> None: + if len(path) < 2: + raise TrajectoryParametrizationError("Planner returned fewer than two waypoints") + expected = list(expected_names) + for waypoint_index, state in enumerate(path): + if list(state.name) != expected: + raise TrajectoryParametrizationError( + f"Waypoint {waypoint_index} joint names do not match selected order" + ) + positions = list(state.position) + if len(positions) != len(expected): + raise TrajectoryParametrizationError( + f"Waypoint {waypoint_index} position dimension mismatch" + ) + cls._assert_finite_sequence( + positions, + f"Waypoint {waypoint_index} positions", + ) + + @classmethod + def _timed_trajectory( + cls, + selection: PlanningGroupSelection, + path: Sequence[JointState], + timestamps: Sequence[float], + ) -> JointTrajectory: + if len(timestamps) != len(path): + raise TrajectoryParametrizationError("Planner must return one timestamp per waypoint") + points: list[TrajectoryPoint] = [] + for waypoint_index, (state, timestamp) in enumerate(zip(path, timestamps, strict=True)): + velocities = list(state.velocity) + if len(velocities) != len(selection.joint_names): + raise TrajectoryParametrizationError( + f"Waypoint {waypoint_index} velocity dimension mismatch" + ) + points.append( + TrajectoryPoint( + time_from_start=float(timestamp), + positions=list(state.position), + velocities=velocities, + ) + ) + return JointTrajectory( + joint_names=list(selection.joint_names), + points=points, + ) + + @classmethod + def _validate_trajectory( + cls, + trajectory: JointTrajectory, + expected_names: Sequence[str], + *, + expected_start: Sequence[float], + expected_goal: Sequence[float], + ) -> None: + expected = list(expected_names) + if list(trajectory.joint_names) != expected: + raise TrajectoryParametrizationError( + "Generated trajectory joint names do not match selected order" + ) + if not trajectory.points: + raise TrajectoryParametrizationError("Generated trajectory has no points") + previous_time: float | None = None + for point_index, point in enumerate(trajectory.points): + if len(point.positions) != len(expected) or len(point.velocities) != len(expected): + raise TrajectoryParametrizationError( + f"Generated point {point_index} dimension mismatch" + ) + cls._assert_finite_sequence( + point.positions, + f"Generated point {point_index} positions", + ) + cls._assert_finite_sequence( + point.velocities, + f"Generated point {point_index} velocities", + ) + if not math.isfinite(point.time_from_start): + raise TrajectoryParametrizationError( + f"Generated point {point_index} time is non-finite" + ) + if point_index == 0 and point.time_from_start != 0.0: + raise TrajectoryParametrizationError("Generated trajectory must start at time 0") + if previous_time is not None and point.time_from_start <= previous_time: + raise TrajectoryParametrizationError( + "Generated trajectory times must be strictly increasing" + ) + previous_time = point.time_from_start + if not cls._positions_close(trajectory.points[0].positions, expected_start): + raise TrajectoryParametrizationError( + "Generated trajectory does not preserve the path start" + ) + if not cls._positions_close(trajectory.points[-1].positions, expected_goal): + raise TrajectoryParametrizationError( + "Generated trajectory does not preserve the path goal" + ) + + @staticmethod + def _positions_close(first: Sequence[float], second: Sequence[float]) -> bool: + return len(first) == len(second) and all( + math.isclose( + left, + right, + rel_tol=0.0, + abs_tol=_TRAJECTORY_POSITION_TOLERANCE, + ) + for left, right in zip(first, second, strict=True) + ) diff --git a/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py new file mode 100644 index 0000000000..4004d80ceb --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/roboplan_toppra_parametrizer.py @@ -0,0 +1,212 @@ +# Copyright 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. + +"""RoboPlan TOPP-RA trajectory parametrization adapter.""" + +from dataclasses import dataclass +import math +import sys +from typing import Any + +import numpy as np +import roboplan.core as roboplan_core +import roboplan.toppra as roboplan_toppra + +from dimos.manipulation.planning.groups.models import PlanningGroupSelection +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.manipulation.planning.trajectory_generator.config import ( + RoboPlanTOPPRAParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + BaseTrajectoryParametrizer, + TrajectoryParametrizationError, +) +from dimos.manipulation.planning.world.roboplan_model import RoboPlanGroup, RoboPlanModel +from dimos.manipulation.planning.world.roboplan_world import RoboPlanWorld +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint + + +@dataclass(frozen=True) +class _GroupParametrizer: + group: RoboPlanGroup + native: Any + + +class RoboPlanTOPPRAParametrizer(BaseTrajectoryParametrizer): + """Convert selected-joint paths with a finalized RoboPlan scene.""" + + def __init__( + self, + config: RoboPlanTOPPRAParametrizationConfig, + ) -> None: + self._config = config + self._groups: dict[frozenset[str], _GroupParametrizer] = {} + + def _parametrize_path( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + path: tuple[JointState, ...], + speed_scale: float, + ) -> JointTrajectory: + if not isinstance(world, RoboPlanWorld): + raise TrajectoryParametrizationError("RoboPlan TOPP-RA requires RoboPlanWorld") + try: + with world.parametrization_model() as model: + resolved = self._resolve_group(model, selection) + native_path = self._native_path(resolved.group, selection, path) + native_trajectory = resolved.native.generate( + native_path, self._options(speed_scale) + ) + return self._canonical_result( + resolved, + selection, + native_trajectory, + ) + except TrajectoryParametrizationError: + raise + except (IndexError, KeyError, RuntimeError, TypeError, ValueError) as exc: + raise TrajectoryParametrizationError( + f"RoboPlan TOPP-RA parametrization failed: {exc}" + ) from exc + + def _resolve_group( + self, + model: RoboPlanModel, + selection: PlanningGroupSelection, + ) -> _GroupParametrizer: + key = frozenset(selection.group_ids) + cached = self._groups.get(key) + if cached is not None: + return cached + group = model.groups.get(key) + if group is None: + raise TrajectoryParametrizationError( + f"RoboPlan has no generated group for {list(selection.group_ids)}" + ) + expected = set(selection.joint_names) + if expected != set(group.public_names): + raise TrajectoryParametrizationError( + f"RoboPlan group '{group.name}' does not match selected joints" + ) + self._validate_limits( + model.scene.getVelocityLimitVectors(group.name), + group, + "velocity", + ) + self._validate_limits( + model.scene.getAccelerationLimitVectors(group.name), + group, + "acceleration", + ) + resolved = _GroupParametrizer( + group=group, + native=roboplan_toppra.PathParameterizerTOPPRA(model.scene, group.name), + ) + self._groups[key] = resolved + return resolved + + @staticmethod + def _validate_limits( + bounds: tuple[Any, Any], + group: RoboPlanGroup, + label: str, + ) -> None: + lower = np.asarray(bounds[0], dtype=np.float64) + upper = np.asarray(bounds[1], dtype=np.float64) + if lower.shape != upper.shape or len(lower) != len(group.native_names): + raise TrajectoryParametrizationError( + f"RoboPlan {label} limits do not match group '{group.name}'" + ) + for public_name, low, high in zip(group.public_names, lower, upper, strict=True): + magnitude = min(abs(float(low)), abs(float(high))) + if not math.isfinite(magnitude) or magnitude <= 0.0 or magnitude >= sys.float_info.max: + raise TrajectoryParametrizationError( + f"RoboPlan group '{group.name}' has no usable URDF {label} " + f"limit for joint '{public_name}'" + ) + + @staticmethod + def _native_path( + group: RoboPlanGroup, + selection: PlanningGroupSelection, + path_states: tuple[JointState, ...], + ) -> Any: + public_index = {name: index for index, name in enumerate(selection.joint_names)} + path = roboplan_core.JointPath() + path.joint_names = list(group.native_names) + path.positions = [ + np.asarray( + [state.position[public_index[public_name]] for public_name in group.public_names], + dtype=np.float64, + ) + for state in path_states + ] + return path + + def _options(self, speed_scale: float) -> Any: + return roboplan_toppra.TOPPRAOptions( + dt=self._config.output_period, + mode={ + "hermite": roboplan_toppra.SplineFittingMode.Hermite, + "cubic": roboplan_toppra.SplineFittingMode.Cubic, + "adaptive": roboplan_toppra.SplineFittingMode.Adaptive, + "linear_blend": roboplan_toppra.SplineFittingMode.LinearBlend, + }[self._config.fitting_mode], + velocity_scale=self._config.velocity_scale * speed_scale, + acceleration_scale=self._config.acceleration_scale * speed_scale, + max_adaptive_iterations=self._config.max_adaptive_iterations, + max_adaptive_step_size=self._config.max_adaptive_step_size, + max_blend_deviation=self._config.max_blend_deviation, + ) + + @staticmethod + def _canonical_result( + resolved: _GroupParametrizer, + selection: PlanningGroupSelection, + native_trajectory: Any, + ) -> JointTrajectory: + native_names = tuple(native_trajectory.joint_names) + if set(native_names) != set(resolved.group.native_names): + raise TrajectoryParametrizationError("RoboPlan TOPP-RA returned unexpected joint names") + native_index = {name: index for index, name in enumerate(native_names)} + native_by_public = dict( + zip( + resolved.group.public_names, + resolved.group.native_names, + strict=True, + ) + ) + output_indices = [native_index[native_by_public[name]] for name in selection.joint_names] + times = [float(value) for value in native_trajectory.times] + positions = list(native_trajectory.positions) + velocities = list(native_trajectory.velocities) + if not (len(times) == len(positions) == len(velocities)): + raise TrajectoryParametrizationError( + "RoboPlan TOPP-RA returned inconsistent trajectory fields" + ) + points = [ + TrajectoryPoint( + time_from_start=time, + positions=[float(position[index]) for index in output_indices], + velocities=[float(velocity[index]) for index in output_indices], + ) + for time, position, velocity in zip(times, positions, velocities, strict=True) + ] + return JointTrajectory( + joint_names=list(selection.joint_names), + points=points, + ) diff --git a/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py new file mode 100644 index 0000000000..f939029693 --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/simple_parametrizer.py @@ -0,0 +1,107 @@ +# Copyright 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. + +"""Compatibility trajectory parametrizer using segmented trapezoids.""" + +import math + +from dimos.manipulation.planning.groups.models import PlanningGroupSelection +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.manipulation.planning.trajectory_generator.config import ( + SimpleTrapezoidParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.joint_trajectory_generator import ( + JointTrajectoryGenerator, +) +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + BaseTrajectoryParametrizer, + TrajectoryParametrizationError, +) +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory + + +class SimpleTrapezoidParametrizer(BaseTrajectoryParametrizer): + """Wrap the existing trajectory generator behind the adapter protocol.""" + + def __init__(self, config: SimpleTrapezoidParametrizationConfig) -> None: + self._config = config + + def _parametrize_path( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + path: tuple[JointState, ...], + speed_scale: float, + ) -> JointTrajectory: + request_velocity_limits, request_acceleration_limits = self._selected_limits( + world, + selection, + ) + velocity_limits = tuple( + value * self._config.velocity_scale * speed_scale for value in request_velocity_limits + ) + acceleration_limits = tuple( + value * self._config.acceleration_scale * speed_scale + for value in request_acceleration_limits + ) + try: + generator = JointTrajectoryGenerator( + num_joints=len(selection.joint_names), + max_velocity=list(velocity_limits), + max_acceleration=list(acceleration_limits), + points_per_segment=self._config.points_per_segment, + ) + generated = generator.generate([list(state.position) for state in path]) + except (IndexError, RuntimeError, TypeError, ValueError) as exc: + raise TrajectoryParametrizationError( + f"Simple trapezoid parametrization failed: {exc}" + ) from exc + return JointTrajectory( + joint_names=list(selection.joint_names), + points=generated.points, + timestamp=generated.timestamp, + ) + + @staticmethod + def _selected_limits( + world: WorldSpec, + selection: PlanningGroupSelection, + ) -> tuple[tuple[float, ...], tuple[float, ...]]: + configs = {} + for robot_id in world.get_robot_ids(): + config = world.get_robot_config(robot_id) + configs[config.name] = config + velocities: list[float] = [] + accelerations: list[float] = [] + for global_name in selection.joint_names: + if "/" not in global_name: + raise TrajectoryParametrizationError(f"Joint '{global_name}' is not globally named") + robot_name, local_name = global_name.split("/", 1) + selected_config = configs.get(robot_name) + if selected_config is None: + raise TrajectoryParametrizationError(f"Unknown robot for joint '{global_name}'") + if local_name not in selected_config.joint_names: + raise TrajectoryParametrizationError(f"Unknown local joint '{global_name}'") + velocity = float(selected_config.max_velocity) + acceleration = float(selected_config.max_acceleration) + if not math.isfinite(velocity) or velocity <= 0.0: + raise TrajectoryParametrizationError(f"Invalid velocity limit for '{global_name}'") + if not math.isfinite(acceleration) or acceleration <= 0.0: + raise TrajectoryParametrizationError( + f"Invalid acceleration limit for '{global_name}'" + ) + velocities.append(velocity) + accelerations.append(acceleration) + return tuple(velocities), tuple(accelerations) diff --git a/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py new file mode 100644 index 0000000000..8d49c58eec --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/test_parametrizer.py @@ -0,0 +1,237 @@ +# Copyright 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. + +"""Contract tests for PlanningResult-to-GeneratedPlan materialization.""" + +from unittest.mock import MagicMock + +import pytest + +from dimos.manipulation.planning.groups.models import ( + PlanningGroup, + PlanningGroupSelection, +) +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.models import PlanningResult +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + BaseTrajectoryParametrizer, + TrajectoryParametrizationError, +) +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory +from dimos.msgs.trajectory_msgs.TrajectoryPoint import TrajectoryPoint + + +class _FixedParametrizer(BaseTrajectoryParametrizer): + def __init__(self, output: JointTrajectory) -> None: + self.output = output + self.calls: list[float] = [] + + def _parametrize_path( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + path: tuple[JointState, ...], + speed_scale: float, + ) -> JointTrajectory: + self.calls.append(speed_scale) + return self.output + + +def _selection() -> PlanningGroupSelection: + return PlanningGroupSelection.from_groups( + ( + PlanningGroup( + id="arm/group", + robot_name="arm", + group_name="group", + joint_names=("arm/a", "arm/b"), + local_joint_names=("a", "b"), + base_link="base", + ), + ) + ) + + +def _path() -> list[JointState]: + names = ["arm/a", "arm/b"] + return [ + JointState(name=names, position=[0.0, 0.0]), + JointState(name=names, position=[0.2, 0.1]), + JointState(name=names, position=[0.4, 0.0]), + ] + + +def _output() -> JointTrajectory: + return JointTrajectory( + joint_names=["arm/a", "arm/b"], + points=[ + TrajectoryPoint( + time_from_start=0.0, + positions=[0.0, 0.0], + velocities=[0.0, 0.0], + ), + TrajectoryPoint( + time_from_start=0.5, + positions=[0.4, 0.0], + velocities=[0.0, 0.0], + ), + ], + ) + + +def test_materializes_trajectory_and_preserves_source_path() -> None: + parametrizer = _FixedParametrizer(_output()) + source_path = _path() + + plan = parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult( + status=PlanningStatus.SUCCESS, + path=source_path, + planning_time=0.2, + iterations=12, + ), + speed_scale=0.4, + ) + + assert [state.position for state in plan.path] == [ + [0.0, 0.0], + [0.2, 0.1], + [0.4, 0.0], + ] + assert plan.path is not source_path + assert plan.trajectory is parametrizer.output + assert plan.planning_time == 0.2 + assert plan.iterations == 12 + assert parametrizer.calls == [0.4] + + +def test_timed_planner_result_bypasses_backend_path_conversion() -> None: + parametrizer = _FixedParametrizer(_output()) + path = [ + JointState( + name=["arm/a", "arm/b"], + position=[0.0, 0.0], + velocity=[0.0, 0.0], + ), + JointState( + name=["arm/a", "arm/b"], + position=[0.4, 0.0], + velocity=[0.3, 0.0], + ), + ] + + plan = parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult( + status=PlanningStatus.SUCCESS, + path=path, + timestamps=[0.0, 0.75], + ), + ) + + assert parametrizer.calls == [] + assert [point.time_from_start for point in plan.trajectory.points] == [ + 0.0, + 0.75, + ] + assert plan.trajectory.points[-1].velocities == [0.3, 0.0] + + +def test_timed_planner_result_requires_velocity_for_each_joint() -> None: + parametrizer = _FixedParametrizer(_output()) + path = _path() + + with pytest.raises(TrajectoryParametrizationError, match="velocity dimension"): + parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult( + status=PlanningStatus.SUCCESS, + path=path, + timestamps=[0.0, 0.5, 1.0], + ), + ) + + +def test_rejects_backend_trajectory_with_nonincreasing_time() -> None: + output = _output() + output.points[-1].time_from_start = 0.0 + parametrizer = _FixedParametrizer(output) + + with pytest.raises(TrajectoryParametrizationError, match="strictly increasing"): + parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult(status=PlanningStatus.SUCCESS, path=_path()), + ) + + +def test_rejects_backend_trajectory_that_changes_path_goal() -> None: + output = _output() + output.points[-1].positions = [0.3, 0.0] + parametrizer = _FixedParametrizer(output) + + with pytest.raises(TrajectoryParametrizationError, match="path goal"): + parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult(status=PlanningStatus.SUCCESS, path=_path()), + ) + + +@pytest.mark.parametrize( + ("path", "message"), + [ + ( + [ + JointState(name=["arm/a", "arm/b"], position=[0.0, 0.0]), + JointState(name=["wrong/a", "wrong/b"], position=[0.4, 0.0]), + ], + "joint names", + ), + ( + [ + JointState(name=["arm/a", "arm/b"], position=[0.0, 0.0]), + JointState(name=["arm/a", "arm/b"], position=[0.4]), + ], + "dimension", + ), + ( + [ + JointState(name=["arm/a", "arm/b"], position=[0.0, 0.0]), + JointState(name=["arm/a", "arm/b"], position=[float("nan"), 0.0]), + ], + "non-finite", + ), + ], +) +def test_rejects_malformed_path_before_invoking_backend( + path: list[JointState], + message: str, +) -> None: + parametrizer = _FixedParametrizer(_output()) + + with pytest.raises(TrajectoryParametrizationError, match=message): + parametrizer.materialize_plan( + MagicMock(spec=WorldSpec), + _selection(), + PlanningResult(status=PlanningStatus.SUCCESS, path=path), + ) + + assert parametrizer.calls == [] diff --git a/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py new file mode 100644 index 0000000000..ee339d0aa9 --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/test_roboplan_toppra_parametrizer.py @@ -0,0 +1,378 @@ +# Copyright 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. + +"""Tests for the RoboPlan TOPP-RA trajectory parametrizer.""" + +from contextlib import contextmanager +from dataclasses import replace +from types import SimpleNamespace + +import numpy as np +import pytest +from pytest_mock import MockerFixture + +pytest.importorskip("roboplan.toppra") + +from dimos.manipulation.planning.groups.models import ( + PlanningGroup, + PlanningGroupSelection, +) +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.models import PlanningResult +from dimos.manipulation.planning.trajectory_generator.config import ( + RoboPlanTOPPRAParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + TrajectoryParametrizationError, +) +from dimos.manipulation.planning.trajectory_generator.roboplan_toppra_parametrizer import ( + RoboPlanTOPPRAParametrizer, +) +from dimos.manipulation.planning.world.roboplan_model import RoboPlanGroup, RoboPlanModel +from dimos.manipulation.planning.world.roboplan_world import RoboPlanWorld +from dimos.msgs.sensor_msgs.JointState import JointState + +pytestmark = pytest.mark.self_hosted + + +class _Scene: + def __init__(self, *, unbounded_acceleration: bool = False) -> None: + self.unbounded_acceleration = unbounded_acceleration + + def getVelocityLimitVectors(self, group_name: str) -> tuple[np.ndarray, np.ndarray]: + assert group_name == "composite" + return np.asarray([-2.0, -1.0]), np.asarray([2.0, 1.0]) + + def getAccelerationLimitVectors(self, group_name: str) -> tuple[np.ndarray, np.ndarray]: + assert group_name == "composite" + maximum = np.finfo(np.float64).max if self.unbounded_acceleration else 4.0 + return np.asarray([-6.0, -maximum]), np.asarray([6.0, maximum]) + + +class _World(RoboPlanWorld): + def __init__(self, model: RoboPlanModel) -> None: + self.model = model + + @contextmanager + def parametrization_model(self): + yield self.model + + +def _model(*, unbounded_acceleration: bool = False) -> RoboPlanModel: + group = RoboPlanGroup( + group_ids=("left/arm", "right/arm"), + name="composite", + native_names=("native_b", "native_a"), + public_names=("right/b", "left/a"), + ) + return RoboPlanModel( + scene=_Scene(unbounded_acceleration=unbounded_acceleration), + groups={frozenset(group.group_ids): group}, + legacy_group_ids={}, + native_joint_by_global={}, + native_link_by_robot={}, + all_group=group, + ) + + +def _selection_and_result( + names: tuple[str, str] = ("left/a", "right/b"), +) -> tuple[PlanningGroupSelection, PlanningResult]: + positions_by_name = { + "left/a": (0.0, 0.3), + "right/b": (0.1, 0.4), + } + groups_by_name = { + "left/a": PlanningGroup( + id="left/arm", + robot_name="left", + group_name="arm", + joint_names=("left/a",), + local_joint_names=("a",), + base_link="base", + ), + "right/b": PlanningGroup( + id="right/arm", + robot_name="right", + group_name="arm", + joint_names=("right/b",), + local_joint_names=("b",), + base_link="base", + ), + } + selection = PlanningGroupSelection.from_groups(tuple(groups_by_name[name] for name in names)) + result = PlanningResult( + status=PlanningStatus.SUCCESS, + path=[ + JointState( + name=list(names), + position=[positions_by_name[name][0] for name in names], + ), + JointState( + name=list(names), + position=[positions_by_name[name][1] for name in names], + ), + ], + ) + return selection, result + + +def test_roboplan_parametrizer_maps_composite_order_and_native_output( + mocker: MockerFixture, +) -> None: + generated = SimpleNamespace( + joint_names=["native_a", "native_b"], + times=[0.0, 0.5], + positions=[np.asarray([0.0, 0.1]), np.asarray([0.3, 0.4])], + velocities=[np.asarray([0.0, 0.0]), np.asarray([0.2, 0.2])], + ) + native = mocker.MagicMock() + native.generate.return_value = generated + constructor = mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA", + return_value=native, + ) + parametrizer = RoboPlanTOPPRAParametrizer( + RoboPlanTOPPRAParametrizationConfig( + velocity_scale=0.5, + acceleration_scale=0.25, + ), + ) + world = _World(_model()) + selection, planning_result = _selection_and_result() + + result = parametrizer.materialize_plan( + world, + selection, + planning_result, + speed_scale=0.5, + ) + + constructor.assert_called_once() + native_path, options = native.generate.call_args.args + assert native_path.joint_names == ["native_b", "native_a"] + assert [row.tolist() for row in native_path.positions] == [ + [0.1, 0.0], + [0.4, 0.3], + ] + assert options.velocity_scale == 0.25 + assert options.acceleration_scale == 0.125 + assert result.trajectory.joint_names == ["left/a", "right/b"] + assert [point.positions for point in result.trajectory.points] == [ + [0.0, 0.1], + [0.3, 0.4], + ] + assert [point.velocities for point in result.trajectory.points] == [ + [0.0, 0.0], + [0.2, 0.2], + ] + assert [state.position for state in planning_result.path] == [ + [0.0, 0.1], + [0.3, 0.4], + ] + + +def test_roboplan_parametrizer_rejects_unbounded_scene_acceleration( + mocker: MockerFixture, +) -> None: + constructor = mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA" + ) + parametrizer = RoboPlanTOPPRAParametrizer( + RoboPlanTOPPRAParametrizationConfig(), + ) + selection, result = _selection_and_result() + + with pytest.raises( + TrajectoryParametrizationError, + match="no usable URDF acceleration limit for joint 'left/a'", + ): + parametrizer.materialize_plan( + _World(_model(unbounded_acceleration=True)), + selection, + result, + ) + + constructor.assert_not_called() + + +def test_cached_group_preserves_each_request_joint_order( + mocker: MockerFixture, +) -> None: + generated = SimpleNamespace( + joint_names=["native_b", "native_a"], + times=[0.0, 0.5], + positions=[np.asarray([0.1, 0.0]), np.asarray([0.4, 0.3])], + velocities=[np.asarray([0.0, 0.0]), np.asarray([0.2, 0.4])], + ) + native = mocker.MagicMock() + native.generate.return_value = generated + constructor = mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA", + return_value=native, + ) + parametrizer = RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()) + world = _World(_model()) + canonical_selection, canonical_result = _selection_and_result() + reversed_selection, reversed_result = _selection_and_result(("right/b", "left/a")) + + canonical = parametrizer.materialize_plan( + world, + canonical_selection, + canonical_result, + ) + reversed_order = parametrizer.materialize_plan( + world, + reversed_selection, + reversed_result, + ) + + constructor.assert_called_once() + assert canonical.trajectory.joint_names == ["left/a", "right/b"] + assert reversed_order.trajectory.joint_names == ["right/b", "left/a"] + + +def test_roboplan_parametrizer_rejects_incompatible_world( + mocker: MockerFixture, +) -> None: + selection, result = _selection_and_result() + + with pytest.raises( + TrajectoryParametrizationError, + match="RoboPlan TOPP-RA requires RoboPlanWorld", + ): + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + mocker.MagicMock(), selection, result + ) + + +def test_roboplan_parametrizer_reports_missing_generated_group() -> None: + selection, result = _selection_and_result() + model = replace(_model(), groups={}) + + with pytest.raises( + TrajectoryParametrizationError, + match=r"RoboPlan has no generated group for \['left/arm', 'right/arm'\]", + ): + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + _World(model), selection, result + ) + + +def test_roboplan_parametrizer_rejects_group_with_different_joints() -> None: + selection, result = _selection_and_result() + model = _model() + mismatched_group = replace(model.all_group, public_names=("left/a", "right/other")) + model = replace( + model, + groups={frozenset(mismatched_group.group_ids): mismatched_group}, + all_group=mismatched_group, + ) + + with pytest.raises( + TrajectoryParametrizationError, + match="does not match selected joints", + ): + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + _World(model), selection, result + ) + + +def test_roboplan_parametrizer_rejects_limit_vector_with_wrong_size( + mocker: MockerFixture, +) -> None: + selection, result = _selection_and_result() + model = _model() + mocker.patch.object( + model.scene, + "getVelocityLimitVectors", + return_value=([-1.0], [1.0]), + ) + + with pytest.raises( + TrajectoryParametrizationError, + match="velocity limits do not match group 'composite'", + ): + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + _World(model), selection, result + ) + + +def test_roboplan_parametrizer_wraps_native_generation_error( + mocker: MockerFixture, +) -> None: + native = mocker.MagicMock() + native.generate.side_effect = RuntimeError("native failure") + mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA", + return_value=native, + ) + selection, result = _selection_and_result() + + with pytest.raises( + TrajectoryParametrizationError, + match="RoboPlan TOPP-RA parametrization failed: native failure", + ) as error: + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + _World(_model()), selection, result + ) + + assert isinstance(error.value.__cause__, RuntimeError) + + +@pytest.mark.parametrize( + ("generated", "message"), + [ + ( + SimpleNamespace( + joint_names=["native_a", "unexpected"], + times=[0.0], + positions=[np.asarray([0.0, 0.1])], + velocities=[np.asarray([0.0, 0.0])], + ), + "returned unexpected joint names", + ), + ( + SimpleNamespace( + joint_names=["native_a", "native_b"], + times=[0.0], + positions=[np.asarray([0.0, 0.1])], + velocities=[], + ), + "returned inconsistent trajectory fields", + ), + ], +) +def test_roboplan_parametrizer_rejects_malformed_native_trajectory( + mocker: MockerFixture, + generated: SimpleNamespace, + message: str, +) -> None: + native = mocker.MagicMock() + native.generate.return_value = generated + mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "roboplan_toppra_parametrizer.roboplan_toppra.PathParameterizerTOPPRA", + return_value=native, + ) + selection, result = _selection_and_result() + + with pytest.raises(TrajectoryParametrizationError, match=message): + RoboPlanTOPPRAParametrizer(RoboPlanTOPPRAParametrizationConfig()).materialize_plan( + _World(_model()), selection, result + ) diff --git a/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py b/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py new file mode 100644 index 0000000000..37c353610b --- /dev/null +++ b/dimos/manipulation/planning/trajectory_generator/test_simple_parametrizer.py @@ -0,0 +1,158 @@ +# Copyright 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. + +"""Tests for the compatibility trajectory parametrizer Spec implementation.""" + +from pathlib import Path +from unittest.mock import MagicMock + +import pytest +from pytest_mock import MockerFixture + +from dimos.manipulation.planning.groups.models import ( + PlanningGroup, + PlanningGroupSelection, +) +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.models import PlanningResult +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.manipulation.planning.trajectory_generator.config import ( + SimpleTrapezoidParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.parametrizer import ( + TrajectoryParametrizationError, +) +from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( + SimpleTrapezoidParametrizer, +) +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState + + +def _selection() -> PlanningGroupSelection: + return PlanningGroupSelection.from_groups( + ( + PlanningGroup( + id="arm/manipulator", + robot_name="arm", + group_name="manipulator", + joint_names=("arm/a", "arm/b"), + local_joint_names=("a", "b"), + base_link="base", + tip_link="tip", + ), + ) + ) + + +def _world(*, velocity: float = 2.0, acceleration: float = 6.0) -> WorldSpec: + config = RobotModelConfig( + name="arm", + model_path=Path("/robot.urdf"), + base_pose=PoseStamped(), + joint_names=["a", "b"], + base_link="base", + max_velocity=velocity, + max_acceleration=acceleration, + ) + world = MagicMock(spec=WorldSpec) + world.get_robot_ids.return_value = ["arm-id"] + world.get_robot_config.return_value = config + return world + + +def _result() -> PlanningResult: + names = ["arm/a", "arm/b"] + return PlanningResult( + status=PlanningStatus.SUCCESS, + path=[ + JointState(name=names, position=[0.0, 0.0]), + JointState(name=names, position=[0.2, 0.1]), + JointState(name=names, position=[0.4, 0.0]), + ], + ) + + +def test_simple_parametrizer_materializes_segmented_trapezoid_plan() -> None: + parametrizer = SimpleTrapezoidParametrizer( + SimpleTrapezoidParametrizationConfig( + velocity_scale=0.5, + acceleration_scale=0.25, + points_per_segment=4, + ) + ) + result = _result() + + plan = parametrizer.materialize_plan( + _world(), + _selection(), + result, + speed_scale=0.5, + ) + + assert plan.group_ids == ("arm/manipulator",) + assert plan.trajectory.joint_names == ["arm/a", "arm/b"] + assert len(plan.trajectory.points) == 9 + assert plan.trajectory.points[0].positions == [0.0, 0.0] + assert plan.trajectory.points[4].positions == [0.2, 0.1] + assert plan.trajectory.points[-1].positions == [0.4, 0.0] + assert [state.position for state in result.path] == [ + [0.0, 0.0], + [0.2, 0.1], + [0.4, 0.0], + ] + + +def test_simple_parametrizer_rejects_invalid_dimos_limits() -> None: + parametrizer = SimpleTrapezoidParametrizer(SimpleTrapezoidParametrizationConfig()) + + with pytest.raises( + TrajectoryParametrizationError, + match="Invalid velocity limit for 'arm/a'", + ): + parametrizer.materialize_plan( + _world(velocity=0.0), + _selection(), + _result(), + ) + + +def test_simple_parametrizer_reports_generator_failure(mocker: MockerFixture) -> None: + generator = mocker.patch( + "dimos.manipulation.planning.trajectory_generator." + "simple_parametrizer.JointTrajectoryGenerator" + ) + generator.return_value.generate.side_effect = RuntimeError("boom") + + with pytest.raises(TrajectoryParametrizationError, match="failed: boom"): + SimpleTrapezoidParametrizer(SimpleTrapezoidParametrizationConfig()).materialize_plan( + _world(), + _selection(), + _result(), + ) + + +@pytest.mark.parametrize( + "speed_scale", + [0.0, 1.01, float("nan")], +) +def test_parametrizer_rejects_invalid_runtime_speed(speed_scale: float) -> None: + with pytest.raises(TrajectoryParametrizationError, match="speed_scale"): + SimpleTrapezoidParametrizer(SimpleTrapezoidParametrizationConfig()).materialize_plan( + _world(), + _selection(), + _result(), + speed_scale=speed_scale, + ) diff --git a/dimos/manipulation/planning/world/roboplan_model.py b/dimos/manipulation/planning/world/roboplan_model.py index 75f5b1d492..3971dbfa64 100644 --- a/dimos/manipulation/planning/world/roboplan_model.py +++ b/dimos/manipulation/planning/world/roboplan_model.py @@ -37,6 +37,8 @@ _ROOT_LINK = "dimos_world" _ROOT_JOINT = "dimos_world_joint" _FREE_ROOTS = {"world", "map", _ROOT_LINK} +# TODO: Remove this global fallback when formal per-joint acceleration overrides are available. +_DEFAULT_ACCELERATION_LIMIT = 2.0 _REFERENCE_ATTRIBUTES = ( "reference", "frame", @@ -178,6 +180,7 @@ def _compose(prepared: Sequence[tuple[_BuildRobot, Path]], composite: bool) -> _ root = ET.parse(path).getroot() if _tag(root.tag) != "robot": raise ValueError(f"Prepared model for '{config.name}' is not a URDF robot") + _add_missing_acceleration_limits(root) mapping = _name_map(root, config.name, composite) mapped_names = { value @@ -234,6 +237,15 @@ def _compose(prepared: Sequence[tuple[_BuildRobot, Path]], composite: bool) -> _ ) +def _add_missing_acceleration_limits(root: ET.Element) -> None: + for joint in root.iter(): + if _tag(joint.tag) != "joint" or joint.get("type") == "fixed": + continue + limit = next((child for child in joint if _tag(child.tag) == "limit"), None) + if limit is not None and limit.get("acceleration") is None: + limit.set("acceleration", str(_DEFAULT_ACCELERATION_LIMIT)) + + def _name_map(root: ET.Element, robot_name: RobotName, prefix: bool) -> _NameMap: def names(tag: str) -> dict[str, str]: return { diff --git a/dimos/manipulation/planning/world/roboplan_world.py b/dimos/manipulation/planning/world/roboplan_world.py index 3f1580c7af..ee918218cb 100644 --- a/dimos/manipulation/planning/world/roboplan_world.py +++ b/dimos/manipulation/planning/world/roboplan_world.py @@ -968,6 +968,12 @@ def _require_model(self) -> RoboPlanModel: raise RuntimeError("RoboPlan model is not initialized; finalize the world first") return self._model + @contextmanager + def parametrization_model(self) -> Generator[RoboPlanModel, None, None]: + """Yield the finalized trajectory model under the world scene lock.""" + with self._lock: + yield self._require_model() + def _full_scene_q( self, ctx: RoboPlanContext, diff --git a/dimos/manipulation/test_generated_plan_materialization.py b/dimos/manipulation/test_generated_plan_materialization.py index ec75185577..1e337bc16f 100644 --- a/dimos/manipulation/test_generated_plan_materialization.py +++ b/dimos/manipulation/test_generated_plan_materialization.py @@ -26,6 +26,12 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import PlanningStatus from dimos.manipulation.planning.spec.models import PlanningResult +from dimos.manipulation.planning.trajectory_generator.config import ( + SimpleTrapezoidParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( + SimpleTrapezoidParametrizer, +) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform @@ -38,18 +44,20 @@ class RecordingGenerator: calls: list[list[list[float]]] = [] limits: tuple[list[float], list[float]] | None = None - fail = False def __init__( - self, num_joints: int, max_velocity: list[float], max_acceleration: list[float] + self, + num_joints: int, + max_velocity: list[float], + max_acceleration: list[float], + points_per_segment: int = 50, ) -> None: self.num_joints = num_joints + self.points_per_segment = points_per_segment RecordingGenerator.limits = (list(max_velocity), list(max_acceleration)) def generate(self, waypoints: list[list[float]]) -> JointTrajectory: RecordingGenerator.calls.append(waypoints) - if RecordingGenerator.fail: - raise RuntimeError("boom") return JointTrajectory( points=[ TrajectoryPoint( @@ -82,21 +90,30 @@ def _robot(name: str, joints: list[str], velocity: float, acceleration: float) - def _module(monkeypatch: pytest.MonkeyPatch, module_factory): RecordingGenerator.calls = [] RecordingGenerator.limits = None - RecordingGenerator.fail = False monkeypatch.setattr( - "dimos.manipulation.manipulation_module.JointTrajectoryGenerator", RecordingGenerator + "dimos.manipulation.planning.trajectory_generator." + "simple_parametrizer.JointTrajectoryGenerator", + RecordingGenerator, ) left = _robot("left", ["a", "b"], 1.0, 2.0) right = _robot("right", ["c"], 3.0, 4.0) module = module_factory() module._robots = { - "left": ("left_id", left, MagicMock()), - "right": ("right_id", right, MagicMock()), + "left": ("left_id", left), + "right": ("right_id", right), } module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() + module._world_monitor.world.get_robot_ids.return_value = ["left_id", "right_id"] + module._world_monitor.world.get_robot_config.side_effect = { + "left_id": left, + "right_id": right, + }.__getitem__ module._world_monitor.planning_groups = PlanningGroupRegistry([left, right]) module._planner = MagicMock() + module._trajectory_parametrizer = SimpleTrapezoidParametrizer( + SimpleTrapezoidParametrizationConfig() + ) module._state = ManipulationState.PLANNING module._planning_epoch = 1 return module @@ -166,89 +183,6 @@ def test_cartesian_plan_preserves_planner_timestamps_and_velocities(monkeypatch, assert request["auxiliary_groups"] == () -@pytest.mark.parametrize( - ("timestamps", "velocities", "message"), - [ - (None, [[0.0, 0.0], [0.1, 0.1]], "one timestamp"), - ([0.0, 0.0], [[0.0, 0.0], [0.1, 0.1]], "strictly increasing"), - ([0.0, 0.1], [[], [0.1, 0.1]], "velocity dimension"), - ], -) -def test_cartesian_plan_rejects_malformed_timed_results( - monkeypatch, module_factory, timestamps, velocities, message -): - module = _module(monkeypatch, module_factory) - module._state = ManipulationState.IDLE - names = ["left/b", "left/a"] - start = JointState(name=names, position=[0.0, 0.0]) - path = [ - JointState(name=names, position=[0.0, 0.0], velocity=velocities[0]), - JointState(name=names, position=[0.2, 0.1], velocity=velocities[1]), - ] - module._world_monitor.current_global_joint_state.return_value = start - module._planner.plan_cartesian_path.return_value = PlanningResult( - status=PlanningStatus.SUCCESS, - path=path, - timestamps=timestamps, - ) - - result = module.generate_cartesian_plan( - { - "left/group": ( - Transform.identity(), - Transform(translation=Vector3(0.01, 0.0, 0.0)), - ) - }, - RoboPlanCartesianPathConfig(), - ) - - assert result is None - assert module._last_plan is None - assert message in module._error_message - - -@pytest.mark.parametrize( - ("path", "message"), - [ - (_path(["left/a", "left/b"], [0.0, 0.0], [1.0, 1.0]), "joint names"), - (_path(["left/b", "left/a"], [0.0], [1.0]), "dimension"), - (_path(["left/b", "left/a"], [0.0, float("nan")], [1.0, 1.0]), "non-finite"), - ], -) -def test_rejects_malformed_or_nonfinite_waypoints(monkeypatch, module_factory, path, message): - module = _module(monkeypatch, module_factory) - module._planner.plan_selected_joint_path.return_value = PlanningResult( - status=PlanningStatus.SUCCESS, path=path - ) - - assert not module._plan_selected_path(("left/group",), path[0], path[-1], 1) - assert module._last_plan is None - assert message in module._error_message - - -def test_rejects_invalid_limits_and_generator_failure_without_caching(monkeypatch, module_factory): - module = _module(monkeypatch, module_factory) - module._robots["left"][1].max_velocity = 0.0 - names = ["left/b", "left/a"] - path = _path(names, [0.0, 0.0], [1.0, 1.0]) - module._planner.plan_selected_joint_path.return_value = PlanningResult( - status=PlanningStatus.SUCCESS, path=path - ) - - assert not module._plan_selected_path(("left/group",), path[0], path[-1], 1) - assert module._last_plan is None - assert RecordingGenerator.calls == [] - - module = _module(monkeypatch, module_factory) - RecordingGenerator.fail = True - module._planner.plan_selected_joint_path.return_value = PlanningResult( - status=PlanningStatus.SUCCESS, path=path - ) - assert not module._plan_selected_path(("left/group",), path[0], path[-1], 1) - assert module._last_plan is None - assert len(RecordingGenerator.calls) == 1 - - def test_zero_generation_after_caching_for_status_and_completion(monkeypatch, module_factory): module = _module(monkeypatch, module_factory) names = ["left/b", "left/a"] diff --git a/dimos/manipulation/test_manipulation_monitor_preview.py b/dimos/manipulation/test_manipulation_monitor_preview.py index 76ad85f0d0..ae69ddb25f 100644 --- a/dimos/manipulation/test_manipulation_monitor_preview.py +++ b/dimos/manipulation/test_manipulation_monitor_preview.py @@ -80,12 +80,11 @@ def _one_joint_config(name: str = "arm") -> RobotModelConfig: def _install_generated_plan( module: ManipulationModule, config: RobotModelConfig, - traj_gen: MagicMock, *points: list[float], ) -> None: """Install a generated plan and enough monitor state to derive robot paths.""" global_joint_names = [f"{config.name}/{joint}" for joint in config.joint_names] - module._robots = {config.name: ("robot_id", config, traj_gen)} + module._robots = {config.name: ("robot_id", config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([config]) module._world_monitor.get_current_joint_state.return_value = JointState( @@ -126,7 +125,7 @@ def _make_module_with_monitor( module._init_joints = {} for config in configs: robot_id = f"robot_{config.name}" - module._robots[config.name] = (robot_id, config, MagicMock()) + module._robots[config.name] = (robot_id, config) return module @@ -300,7 +299,7 @@ def test_multi_robot_splits_correctly(self, module_factory): def test_no_monitor_returns_early(self, robot_config_with_mapping, module_factory): """When world_monitor is None, _on_joint_state returns without error.""" module = module_factory() - module._robots = {"left_arm": ("id", robot_config_with_mapping, MagicMock())} + module._robots = {"left_arm": ("id", robot_config_with_mapping)} module._world_monitor = None # Should not raise @@ -397,8 +396,7 @@ def test_dismiss_preview_routes_to_monitor(self, module_factory): def test_preview_routes_one_complete_plan_with_default_duration(self, module_factory): module = module_factory() config = _one_joint_config() - traj_gen = MagicMock() - _install_generated_plan(module, config, traj_gen, [0.0], [2.0]) + _install_generated_plan(module, config, [0.0], [2.0]) assert module.preview_plan() is True @@ -410,10 +408,9 @@ def test_preview_robot_name_validates_affectedness_without_trimming(self, module module = module_factory() left = _one_joint_config("left") right = _one_joint_config("right") - traj_gen = MagicMock() module._robots = { - "left": ("left_id", left, traj_gen), - "right": ("right_id", right, traj_gen), + "left": ("left_id", left), + "right": ("right_id", right), } module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([left, right]) @@ -436,8 +433,7 @@ def test_preview_robot_name_validates_affectedness_without_trimming(self, module def test_preview_rejects_unaffected_compatibility_robot(self, module_factory): module = module_factory() config = _one_joint_config() - traj_gen = MagicMock() - _install_generated_plan(module, config, traj_gen, [0.0], [1.0]) + _install_generated_plan(module, config, [0.0], [1.0]) assert module.preview_plan(robot_name="other") is False module._world_monitor.animate_trajectory.assert_not_called() diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 2bcc8a9afc..dab99dd2b4 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -17,7 +17,7 @@ from __future__ import annotations from pathlib import Path -from unittest.mock import MagicMock +from unittest.mock import ANY, MagicMock import pytest from pytest_mock import MockerFixture @@ -45,6 +45,12 @@ IKResult, PlanningResult, ) +from dimos.manipulation.planning.trajectory_generator.config import ( + SimpleTrapezoidParametrizationConfig, +) +from dimos.manipulation.planning.trajectory_generator.simple_parametrizer import ( + SimpleTrapezoidParametrizer, +) from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -130,12 +136,11 @@ def _one_joint_config(name: str = "arm") -> RobotModelConfig: def _install_generated_plan( module: ManipulationModule, config: RobotModelConfig, - traj_gen: MagicMock, *points: list[float], ) -> None: """Install a generated plan and enough monitor state to derive robot paths.""" global_joint_names = [f"{config.name}/{joint}" for joint in config.joint_names] - module._robots = {config.name: ("robot_id", config, traj_gen)} + module._robots = {config.name: ("robot_id", config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([config]) module._world_monitor.get_current_joint_state.return_value = JointState( @@ -196,6 +201,12 @@ def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: ) +def _enable_simple_parametrization(module: ManipulationModule) -> None: + module._trajectory_parametrizer = SimpleTrapezoidParametrizer( + SimpleTrapezoidParametrizationConfig() + ) + + class TestObstacleUpdates: def test_complete_update_forwards_new_obstacle_value(self, module_factory) -> None: module = module_factory() @@ -310,7 +321,7 @@ def test_cancel_hides_active_plan_preview(self, module_factory): def test_cancel_completed_execution_cancels_coordinator_task(self, module_factory): module = module_factory() config = _one_joint_config() - _install_generated_plan(module, config, MagicMock(), [0.0], [0.1]) + _install_generated_plan(module, config, [0.0], [0.1]) coordinator = _control_coordinator(cancel_status=TrajectoryCancellationStatus.CANCELLED) module._control_coordinator = coordinator module._initialize_execution() @@ -348,11 +359,33 @@ def test_fail_sets_fault_state(self, module_factory): assert module._state == ManipulationState.FAULT assert module._error_message == "Test error" + def test_motion_speed_applies_to_future_plans_only(self, module_factory): + module = module_factory() + accepted = GeneratedPlan( + trajectory=JointTrajectory(), + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j0"], position=[0.0])], + ) + module._last_plan = accepted + + assert module.set_motion_speed(0.5) is True + assert module.get_motion_speed() == pytest.approx(0.5) + assert module._last_plan is accepted + + @pytest.mark.parametrize("invalid", [0.0, 1.01, float("nan")]) + def test_motion_speed_rejects_invalid_values(self, module_factory, invalid: float): + module = module_factory() + assert module.set_motion_speed(0.5) is True + + assert module.set_motion_speed(invalid) is False + assert module.get_motion_speed() == pytest.approx(0.5) + assert "motion speed scale" in module.get_error() + def test_begin_planning_state_checks(self, robot_config, module_factory): """_begin_planning only allowed from IDLE or COMPLETED.""" module = module_factory() module._world_monitor = MagicMock() - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} # From IDLE - OK module._state = ManipulationState.IDLE @@ -374,7 +407,7 @@ class TestRobotSelection: def test_single_robot_default(self, robot_config, module_factory): """Single robot is used by default.""" module = module_factory() - module._robots = {"arm": ("id", robot_config, MagicMock())} + module._robots = {"arm": ("id", robot_config)} result = module._get_robot() assert result is not None @@ -384,8 +417,8 @@ def test_multiple_robots_require_name(self, robot_config, module_factory): """Multiple robots require explicit name.""" module = module_factory() module._robots = { - "left": ("id1", robot_config, MagicMock()), - "right": ("id2", robot_config, MagicMock()), + "left": ("id1", robot_config), + "right": ("id2", robot_config), } # No name - fails @@ -406,6 +439,7 @@ def __init__(self, mocker: MockerFixture) -> None: world_monitor=self.mock_world_monitor, planner=MagicMock(), kinematics=MagicMock(), + trajectory_parametrizer=MagicMock(), ) self.mock_planning_specs = mocker.patch( "dimos.manipulation.manipulation_module.create_planning_specs", @@ -416,7 +450,6 @@ def __init__(self, mocker: MockerFixture) -> None: return_value=self.mock_world, ) mocker.patch("dimos.manipulation.manipulation_module.create_manipulation_visualization") - mocker.patch("dimos.manipulation.manipulation_module.JointTrajectoryGenerator") @pytest.fixture @@ -468,6 +501,7 @@ def test_kinematics_config_is_passed_to_factory( planner=module.config.planner, kinematics_name=None, kinematics=kinematics, + trajectory_parametrization=ANY, ) def test_legacy_kinematics_name_still_selects_backend( @@ -491,6 +525,7 @@ def test_legacy_kinematics_name_still_selects_backend( planner=module.config.planner, kinematics_name="pink", kinematics=module.config.kinematics, + trajectory_parametrization=ANY, ) def test_nested_kinematics_config_parses_cli_override_shape(self) -> None: @@ -512,7 +547,7 @@ def test_nested_kinematics_config_parses_cli_override_shape(self) -> None: def test_solve_ik_rpc_calls_configured_backend(self, robot_config, module_factory): """solve_ik returns the backend IKResult without path planning.""" module = module_factory() - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) @@ -553,7 +588,7 @@ def test_solve_ik_rpc_calls_configured_backend(self, robot_config, module_factor def test_solve_ik_rpc_returns_failure_without_joint_state(self, robot_config, module_factory): """solve_ik reports a failed IKResult when no seed state is available.""" module = module_factory() - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) module._world_monitor.current_global_joint_state.return_value = JointState( @@ -574,7 +609,7 @@ def test_solve_ik_rpc_accepts_explicit_seed_without_current_state( ): """solve_ik succeeds with an explicit seed when no current state is available.""" module = module_factory() - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) @@ -599,7 +634,7 @@ class TestPlanningGroupApis: def test_list_planning_groups_and_robot_info_include_groups(self, robot_config, module_factory): module = module_factory() registry = PlanningGroupRegistry([robot_config]) - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = registry module._init_joints = {} @@ -618,13 +653,12 @@ def test_plan_to_joint_targets_stores_generated_plan_and_legacy_caches( ): module = module_factory() registry = PlanningGroupRegistry([robot_config]) - traj_gen = MagicMock() - traj_gen.generate.return_value = _make_trajectory( - (0.0, [0.0, 0.0, 0.0]), (1.0, [0.1, 0.2, 0.3]) - ) - module._robots = {"test_arm": ("robot_id", robot_config, traj_gen)} + module._robots = {"test_arm": ("robot_id", robot_config)} + _enable_simple_parametrization(module) module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() + module._world_monitor.world.get_robot_ids.return_value = ["robot_id"] + module._world_monitor.world.get_robot_config.return_value = robot_config module._world_monitor.planning_groups = registry module._world_monitor.current_global_joint_state.return_value = JointState( name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], @@ -702,13 +736,12 @@ def test_plan_to_pose_targets_uses_group_ik_and_selected_path( ): module = module_factory() registry = PlanningGroupRegistry([robot_config]) - traj_gen = MagicMock() - traj_gen.generate.return_value = _make_trajectory( - (0.0, [0.0, 0.0, 0.0]), (1.0, [0.1, 0.2, 0.3]) - ) - module._robots = {"test_arm": ("robot_id", robot_config, traj_gen)} + module._robots = {"test_arm": ("robot_id", robot_config)} + _enable_simple_parametrization(module) module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() + module._world_monitor.world.get_robot_ids.return_value = ["robot_id"] + module._world_monitor.world.get_robot_config.return_value = robot_config module._world_monitor.planning_groups = registry module._world_monitor.current_global_joint_state.return_value = JointState( name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], @@ -764,7 +797,7 @@ def test_plan_to_pose_targets_uses_group_ik_and_selected_path( def test_failed_plan_materialization_clears_generated_plan(self, robot_config, module_factory): module = module_factory() registry = PlanningGroupRegistry([robot_config]) - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() module._world_monitor.planning_groups = registry @@ -840,8 +873,8 @@ def test_execute_plan_dispatches_selected_subsets_once_with_shared_clock_and_map ) module = module_factory() module._robots = { - "left": ("left_id", left, MagicMock()), - "right": ("right_id", right, MagicMock()), + "left": ("left_id", left), + "right": ("right_id", right), } module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([left, right]) @@ -906,7 +939,7 @@ def test_pose_wrappers_fail_safely_without_unique_pose_group( ], ) module = module_factory() - module._robots = {"test_arm": ("robot_id", no_pose_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", no_pose_config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([no_pose_config]) module._world_monitor.get_ee_pose.side_effect = ValueError("no pose group") @@ -945,7 +978,7 @@ def test_pose_wrappers_fail_safely_with_multiple_pose_groups( ], ) module = module_factory() - module._robots = {"test_arm": ("robot_id", multi_pose_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", multi_pose_config)} module._world_monitor = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([multi_pose_config]) module._world_monitor.get_ee_pose.side_effect = ValueError("multiple pose groups") @@ -962,7 +995,7 @@ def test_pose_wrappers_fail_safely_with_multiple_pose_groups( def test_solve_ik_preserves_backend_failure_detail(self, robot_config, module_factory): """IK diagnostics include the backend's human-readable failure message.""" module = module_factory() - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} module._world_monitor = MagicMock() module._world_monitor.world = MagicMock() module._world_monitor.planning_groups = PlanningGroupRegistry([robot_config]) @@ -997,7 +1030,7 @@ def test_planner_failure_preserves_backend_detail(self, robot_config, module_fac status=PlanningStatus.TIMEOUT, message="planner timed out" ) - module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + module._robots = {"test_arm": ("robot_id", robot_config)} assert not module.plan_to_joints( JointState(position=[1.0, 1.0, 1.0]), robot_name="test_arm" ) @@ -1012,7 +1045,7 @@ class TestExecute: def test_execute_requires_trajectory(self, robot_config, module_factory): """Execute fails without planned trajectory.""" module = module_factory() - module._robots = {"test_arm": ("id", robot_config, MagicMock())} + module._robots = {"test_arm": ("id", robot_config)} assert module.execute() is False assert module._state == ManipulationState.IDLE diff --git a/dimos/manipulation/test_plan_execution.py b/dimos/manipulation/test_plan_execution.py index 5a14c224bc..cc8de4296b 100644 --- a/dimos/manipulation/test_plan_execution.py +++ b/dimos/manipulation/test_plan_execution.py @@ -81,7 +81,7 @@ def _module_with_coordinator( ) ], ) - module._robots = {"arm": ("arm_id", config, MagicMock())} + module._robots = {"arm": ("arm_id", config)} module._initialize_execution() return module @@ -102,11 +102,20 @@ def test_execute_plan_can_dispatch_cached_plan_repeatedly( ) -> None: coordinator = _coordinator() module = _module_with_coordinator(coordinator, module_factory) - module._last_plan = _plan() + plan = _plan() + module._last_plan = plan assert module.execute_plan() assert module.execute_plan() assert coordinator.execute_trajectory.call_count == 2 + for call in coordinator.execute_trajectory.call_args_list: + dispatched = call.args[0] + assert [point.time_from_start for point in dispatched.points] == [ + point.time_from_start for point in plan.trajectory.points + ] + assert [point.velocities for point in dispatched.points] == [ + point.velocities for point in plan.trajectory.points + ] def test_direct_plan_does_not_replace_cached_plan(module_factory) -> None: diff --git a/dimos/manipulation/test_planning_factory.py b/dimos/manipulation/test_planning_factory.py index 2613363c29..ce8e388b85 100644 --- a/dimos/manipulation/test_planning_factory.py +++ b/dimos/manipulation/test_planning_factory.py @@ -21,6 +21,7 @@ import sys from types import ModuleType from typing import Any +from unittest.mock import ANY import pytest from pytest_mock import MockerFixture @@ -29,6 +30,7 @@ from dimos.manipulation.planning.factory import ( create_kinematics, create_planner, + create_planning_specs, create_planning_stack, create_world, validate_backend_combination, @@ -46,6 +48,10 @@ from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.protocols import PlannerSpec +from dimos.manipulation.planning.trajectory_generator.config import ( + SimpleTrapezoidParametrizationConfig, + TrajectoryParametrizationConfig, +) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -125,6 +131,64 @@ def test_validate_backend_combination_rejects_invalid_combinations() -> None: ): validate_backend_combination(world_backend="roboplan", kinematics_name="drake_optimization") + with pytest.raises( + ValueError, + match='trajectory_parametrization.backend="roboplan_toppra" requires', + ): + validate_backend_combination( + world_backend="drake", + planner_backend="rrt_connect", + trajectory_parametrization_backend="roboplan_toppra", + ) + + +@pytest.mark.parametrize( + ("world_backend", "planner", "configured", "expected_backend"), + [ + ("roboplan", RoboPlanPlannerConfig(), None, "roboplan_toppra"), + ("drake", RRTConnectPlannerConfig(), None, "simple_trapezoid"), + ( + "roboplan", + RoboPlanPlannerConfig(), + SimpleTrapezoidParametrizationConfig(), + "simple_trapezoid", + ), + ], +) +def test_create_planning_specs_selects_world_default_unless_overridden( + mocker: MockerFixture, + world_backend: str, + planner: RoboPlanPlannerConfig | RRTConnectPlannerConfig, + configured: TrajectoryParametrizationConfig | None, + expected_backend: str, +) -> None: + world = mocker.MagicMock() + trajectory_parametrizer = mocker.MagicMock() + mocker.patch( + "dimos.manipulation.planning.factory.create_kinematics", + return_value=mocker.MagicMock(), + ) + mocker.patch( + "dimos.manipulation.planning.factory.create_planner", + return_value=mocker.MagicMock(), + ) + create_parametrizer = mocker.patch( + "dimos.manipulation.planning.factory.create_trajectory_parametrizer", + return_value=trajectory_parametrizer, + ) + + result = create_planning_specs( + world=world, + world_backend=world_backend, + planner=planner, + trajectory_parametrization=configured, + ) + + selected = create_parametrizer.call_args.args[0] + assert selected.backend == expected_backend + create_parametrizer.assert_called_once_with(selected, world_backend=world_backend) + assert result.trajectory_parametrizer is trajectory_parametrizer + def test_create_planner_uses_roboplan_world_as_native_planner(mocker: MockerFixture) -> None: world = mocker.MagicMock(spec=PlannerSpec) @@ -178,6 +242,10 @@ def test_create_planning_stack_defaults_to_roboplan( "dimos.manipulation.planning.factory.create_planner", return_value=planner, ) + mocker.patch( + "dimos.manipulation.planning.factory.create_trajectory_parametrizer", + return_value=mocker.MagicMock(name="trajectory_parametrizer"), + ) result = create_planning_stack(robot_config) @@ -230,6 +298,7 @@ def test_start_uses_configured_planner_and_kinematics( world_monitor=world_monitor, planner=planner, kinematics=kinematics, + trajectory_parametrizer=mocker.MagicMock(name="trajectory_parametrizer"), ) create_world_mock = mocker.patch( "dimos.manipulation.manipulation_module.create_world", return_value=world @@ -238,7 +307,6 @@ def test_start_uses_configured_planner_and_kinematics( "dimos.manipulation.manipulation_module.create_planning_specs", return_value=planning_specs, ) - module._initialize_planning() create_world_mock.assert_called_once_with( @@ -250,6 +318,7 @@ def test_start_uses_configured_planner_and_kinematics( planner=planner_config, kinematics_name=None, kinematics=module.config.kinematics, + trajectory_parametrization=ANY, ) assert module._planner is planner assert module._kinematics is kinematics diff --git a/dimos/manipulation/test_roboplan_world.py b/dimos/manipulation/test_roboplan_world.py index 57e437041f..7accf08161 100644 --- a/dimos/manipulation/test_roboplan_world.py +++ b/dimos/manipulation/test_roboplan_world.py @@ -1930,6 +1930,30 @@ def test_scene_receives_generated_model_contents_inline( assert world._scene.constructor_kwargs["package_paths"] == [] +def test_composed_model_fills_only_missing_acceleration_limits( + fake_roboplan: None, robot_config: RobotModelConfig +) -> None: + tree = ET.parse(robot_config.model_path) + authored = tree.find("./joint[@name='joint1']/limit") + assert authored is not None + authored.set("acceleration", "3.5") + tree.write(robot_config.model_path) + + world, _ = _make_world(fake_roboplan, robot_config) + + urdf = ET.fromstring(world._scene.constructor_kwargs["urdf"]) + acceleration_by_joint = { + joint.get("name"): limit.get("acceleration") + for joint in urdf.findall("./joint") + if (limit := joint.find("./limit")) is not None + } + assert acceleration_by_joint == { + "joint1": "3.5", + "joint2": "2.0", + "joint3": "2.0", + } + + def test_base_pose_is_written_to_composed_model( fake_roboplan: None, robot_config: RobotModelConfig ) -> None: diff --git a/dimos/manipulation/visualization/operator.py b/dimos/manipulation/visualization/operator.py index ac0ef65609..ebb1b67070 100644 --- a/dimos/manipulation/visualization/operator.py +++ b/dimos/manipulation/visualization/operator.py @@ -96,6 +96,14 @@ def status(self) -> OperatorStatus: has_plan=self._module.has_planned_path(), ) + def get_motion_speed(self) -> float: + """Return the runtime speed reduction used for future plans.""" + return self._module.get_motion_speed() + + def set_motion_speed(self, speed_scale: float) -> bool: + """Set the runtime speed reduction used for future plans.""" + return self._module.set_motion_speed(speed_scale) + def get_init_joints(self, robot_name: RobotName) -> JointState | None: """Return the operator-authoritative init joint state for a robot.""" init = self._module.get_init_joints(robot_name) diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 02145d9fc5..5695fb85cd 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -15,6 +15,7 @@ from __future__ import annotations from collections.abc import Mapping, MutableMapping, Sequence +import math from typing import TypeAlias, cast from dimos.manipulation.planning.groups.models import PlanningGroup @@ -78,6 +79,7 @@ | GuiDropdownHandle[str] | GuiButtonHandle | GuiCheckboxHandle + | GuiSliderHandle[float] | TransformControlsHandle ) @@ -324,10 +326,14 @@ def plan_cartesian( ) for group_id, pose in pose_targets.items() } + speed_scale = self.operator.get_motion_speed() plan = self.operator.plan_cartesian( CartesianTargetRequest( stamped, - RoboPlanCartesianPathConfig(), + RoboPlanCartesianPathConfig( + velocity_scale=speed_scale, + acceleration_scale=speed_scale, + ), tuple(auxiliary_group_ids), ) ) @@ -394,6 +400,7 @@ def _build_panel_controls(self, gui: GuiApi) -> None: self._handles["preset"] = preset_dropdown self._handles["target_summary"] = gui.add_markdown("Feasibility: `unknown`") self._handles["actions_heading"] = gui.add_markdown("### Actions") + self._build_motion_settings(gui) planning_mode = gui.add_dropdown( "Planning mode", options=list(PLANNING_MODES_BY_LABEL), @@ -421,6 +428,43 @@ def _build_panel_controls(self, gui: GuiApi) -> None: self._handles["joint_control_folder"] = joint_controls self._build_joint_sliders() + def _build_motion_settings(self, gui: GuiApi) -> None: + """Build controls that affect trajectories generated in the future.""" + speed_slider = gui.add_slider( + "Next plan speed", + min=0.05, + max=1.0, + step=0.05, + initial_value=self._motion_speed_scale_for_slider(), + ) + speed_slider.on_update(lambda event: self._set_next_plan_speed(event.target.value)) + self._handles["next_plan_speed"] = speed_slider + + def _motion_speed_scale_for_slider(self) -> float: + """Return the module's speed setting bounded to the slider range.""" + try: + speed_scale = float(self.operator.get_motion_speed()) + except Exception: + logger.warning("Could not read manipulation motion speed", exc_info=True) + return 1.0 + if not math.isfinite(speed_scale) or speed_scale <= 0.0: + return 1.0 + return min(max(speed_scale, 0.05), 1.0) + + def _set_next_plan_speed(self, speed_scale: float) -> None: + """Update future-plan speed without invalidating the accepted plan.""" + if self._closed: + return + if self.state.action_status != ActionStatus.IDLE: + self._set_recoverable_error( + "Cannot change next-plan speed while an operation is active" + ) + return + if not self.operator.set_motion_speed(float(speed_scale)): + self._set_error(self.get_error() or "Invalid next-plan speed") + return + self.refresh() + def _sync_group_selector(self, groups: list[PlanningGroup]) -> None: """Render source-order group toggle buttons without a robot dropdown.""" selected = set(self.state.selected_group_ids) @@ -1119,6 +1163,7 @@ def _update_status_text(self) -> None: ) def _update_control_state(self) -> None: + self._set_disabled("next_plan_speed", self.state.action_status != ActionStatus.IDLE) self._set_disabled("plan", not self.state.can_plan()) self._set_disabled("preview", not self.state.can_preview()) self._set_disabled( @@ -1463,7 +1508,7 @@ def _set_handle_value(self, key: str, value: str) -> None: def _set_disabled(self, key: str, disabled: bool) -> None: handle = self._handles.get(key) - if isinstance(handle, GuiButtonHandle): + if handle is not None and hasattr(handle, "disabled"): self._set_optional_handle_attr(handle, "disabled", disabled) def _set_visible(self, key: str, visible: bool) -> None: diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 8e1428214f..78ba20cf22 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -61,6 +61,7 @@ ViserManipulationScene, ) from dimos.manipulation.visualization.viser.state import ( + ActionStatus, PanelPlanState, PlanningMode, PlanStatus, @@ -217,6 +218,8 @@ def __init__(self, groups: list[PlanningGroup], states: dict[str, JointState]) - self.cancelled = 0 self.cleared = 0 self.last_plan: GeneratedPlan | None = None + self.motion_speed = 1.0 + self.motion_speed_updates: list[float] = [] def make_plan(self, group_ids: tuple[str, ...]) -> GeneratedPlan: names = [ @@ -263,6 +266,14 @@ def get_state(self) -> str: def get_error(self) -> str: return self.error + def get_motion_speed(self) -> float: + return self.motion_speed + + def set_motion_speed(self, speed_scale: float) -> bool: + self.motion_speed = float(speed_scale) + self.motion_speed_updates.append(float(speed_scale)) + return True + def reset(self) -> SimpleNamespace: return SimpleNamespace(is_success=lambda: True) @@ -321,6 +332,12 @@ def status(self) -> SimpleNamespace: has_plan=True, ) + def get_motion_speed(self) -> float: + return self.module.get_motion_speed() + + def set_motion_speed(self, speed_scale: float) -> bool: + return self.module.set_motion_speed(speed_scale) + def get_init_joints(self, robot_name: str) -> JointState | None: return self.module.get_init_joints(robot_name) @@ -514,11 +531,17 @@ def test_panel_contract_group_order_defaults_and_controls( assert server.gui.dropdowns[0].options == ["Select preset...", "Init", "Current", "Home"] assert server.gui.dropdowns[1].options == ["Joint space", "Cartesian space"] assert [ - (slider.label, slider.min, slider.max, slider.value) for slider in server.gui.sliders + (slider.label, slider.min, slider.max, slider.value) + for slider in server.gui.sliders + if slider.label != "Next plan speed" ] == [("arm/manipulator/j1", -1.0, 1.0, 0.1)] server.gui.buttons[1].callback(SimpleNamespace()) assert gui.state.selected_group_ids == ("arm/manipulator", "arm/gripper") - assert [slider.label for slider in server.gui.sliders if not slider.removed] == [ + assert [ + slider.label + for slider in server.gui.sliders + if not slider.removed and slider.label != "Next plan speed" + ] == [ "arm/manipulator/j1", "arm/gripper/j2", ] @@ -704,13 +727,19 @@ def test_valid_init_preset_builds_sliders_after_incomplete_initial_telemetry( ) assert gui.state.group_joint_targets == {} - assert server.gui.sliders == [] + assert [ + slider.label for slider in server.gui.sliders if slider.label != "Next plan speed" + ] == [] module.configs["arm"].home_joints = [-0.5, -1.0] gui._apply_preset("Init") assert gui.state.group_joint_targets[selected.id].position == [-0.5, -1.0] - assert [slider.label for slider in server.gui.sliders if not slider.removed] == [ + assert [ + slider.label + for slider in server.gui.sliders + if not slider.removed and slider.label != "Next plan speed" + ] == [ "arm/manipulator/j1", "arm/manipulator/j2", ] @@ -894,6 +923,7 @@ def test_panel_preset_defaults_and_joint_slider_limits( assert [ (slider.label, slider.min, slider.max, slider.step, slider.value) for slider in server.gui.sliders + if slider.label != "Next plan speed" ] == [ ("arm/manipulator/j1", -1.0, 1.0, 0.001, 0.1), ("arm/manipulator/j2", -2.0, 2.0, 0.001, 0.2), @@ -966,6 +996,42 @@ def test_panel_action_controls_are_present_in_source_order( ] +def test_next_plan_speed_slider_updates_future_speed_without_staling_plan( + panel: Callable[..., tuple[ViserPanelGui, Module, Server]], +) -> None: + selected = group("arm", "manipulator", ("j1",), pose=True) + gui, module, server = panel([selected], states("arm")) + accepted = module.make_plan((selected.id,)) + gui.state.plan_state = PanelPlanState(status=PlanStatus.FRESH, plan=accepted) + speed_slider = next( + slider for slider in server.gui.sliders if slider.label == "Next plan speed" + ) + speed_slider.value = 0.5 + assert speed_slider.callback is not None + + speed_slider.callback(SimpleNamespace(target=speed_slider)) + + assert module.motion_speed_updates == [0.5] + assert module.last_plan is accepted + assert gui.state.plan_state.plan is accepted + assert gui.state.plan_state.status == PlanStatus.FRESH + + +def test_next_plan_speed_slider_is_disabled_during_panel_operation( + panel: Callable[..., tuple[ViserPanelGui, Module, Server]], +) -> None: + selected = group("arm", "manipulator", ("j1",), pose=True) + gui, _module, server = panel([selected], states("arm")) + speed_slider = next( + slider for slider in server.gui.sliders if slider.label == "Next plan speed" + ) + + gui.state.action_status = ActionStatus.RUNNING + gui.refresh() + + assert speed_slider.disabled is True + + def test_target_callbacks_require_current_target_identity( panel: Callable[..., tuple[ViserPanelGui, Module, Server]], ) -> None: diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index 4f9df72a86..076546584c 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -474,12 +474,39 @@ coordinator_yourarm = ControlCoordinator.blueprint( ## Step 4: Add URDF and Planning Integration (Optional) -If you want motion planning (collision-free trajectories via Drake), you need a URDF and a planning blueprint. Add these to your robot's own `blueprints.py`. +If you want motion planning, you need a URDF and a planning blueprint. Add these +to your robot's own `blueprints.py`. ### 4a. Add your URDF Place your URDF/xacro files under LFS data so they can be resolved via `LfsPath`. `LfsPath` is a `Path` subclass that lazily downloads LFS data on first access — this avoids downloading at import time when the blueprint module is loaded. +If the planning blueprint selects the RoboPlan TOPP-RA trajectory +parametrizer, DimOS currently pins RoboPlan to `0.5.1`. Every movable joint in +each selected planning group must provide finite, positive velocity limits. +Authored extended acceleration limits take precedence; when absent, DimOS +temporarily inserts a global `2.0 rad/s²` acceleration fallback during RoboPlan +model composition: + +```xml + + + + +``` + +RoboPlan loads both limits from its scene model. If either is absent, zero, +negative, or non-finite, plan materialization fails before preview or execution +and identifies the affected joint. DimOS does not substitute +`RobotModelConfig.max_velocity`, `velocity_limits`, or `max_acceleration` for +this backend. Formal per-joint DimOS overrides will be added separately. + ```python skip from dimos.utils.data import LfsPath from dimos.manipulation.manipulation_module import manipulation_module @@ -551,12 +578,34 @@ yourarm_planner = manipulation_module( robots=[_make_yourarm_config("arm")], planning_timeout=10.0, visualization={"backend": "meshcat"}, + trajectory_parametrization={"backend": "simple_trapezoid"}, ) # The planner's `coordinator_joint_state` input auto-connects to the # ControlCoordinator's output on the default `/coordinator_joint_state` # topic, so no `.transports(...)` override is needed. ``` +You may omit `trajectory_parametrization` when the world-based default is +appropriate: `world_backend="roboplan"` selects `roboplan_toppra`, while +`world_backend="drake"` selects `simple_trapezoid`. + +To configure TOPP-RA tuning explicitly, select RoboPlan for the world and +parametrizer after adding the URDF limits described above: + +```python skip +yourarm_planner = manipulation_module( + robots=[_make_yourarm_config("arm")], + world_backend="roboplan", + trajectory_parametrization={ + "backend": "roboplan_toppra", + "fitting_mode": "linear_blend", + "velocity_scale": 0.8, + "acceleration_scale": 0.8, + }, + visualization={"backend": "viser"}, +) +``` + ### Key config fields | Field | Description | diff --git a/docs/capabilities/manipulation/index.md b/docs/capabilities/manipulation/index.md index a0aa40f927..a671c46cdf 100644 --- a/docs/capabilities/manipulation/index.md +++ b/docs/capabilities/manipulation/index.md @@ -115,6 +115,48 @@ request. For example, `planner.backend=roboplan` requires `world_backend=roboplan`, and `kinematics.backend=drake_optimization` requires `world_backend=drake`. +Trajectory parametrization is a separate startup choice. Joint-space planners +normally return an untimed geometric path; DimOS accepts the plan only after +the selected backend converts that path to a validated timed trajectory: + +```bash +# Stock xArm compatibility test: independent trapezoids on RoboPlanWorld +dimos run xarm7-planner-coordinator \ + -o manipulationmodule.trajectory_parametrization.backend=simple_trapezoid + +# Omitting trajectory_parametrization selects TOPP-RA for RoboPlanWorld +dimos run xarm7-planner-coordinator + +# Equivalent explicit TOPP-RA selection +dimos run xarm7-planner-coordinator \ + -o manipulationmodule.world_backend=roboplan \ + -o manipulationmodule.trajectory_parametrization.backend=roboplan_toppra \ + -o manipulationmodule.trajectory_parametrization.fitting_mode=linear_blend + +# DrakeWorld selects simple_trapezoid when no parametrizer is specified +dimos run xarm7-planner-coordinator \ + -o manipulationmodule.world_backend=drake \ + -o manipulationmodule.planner.backend=rrt_connect +``` + +Exactly one backend is constructed for the stack lifetime. There is no +cross-backend fallback. `roboplan_toppra` may parametrize paths from either +RoboPlan's planner or the generic RRT planner, but it requires +`world_backend=roboplan` because it reuses that world's model, groups, and URDF +motion limits. A planner-native result that already has timestamps and +velocities bypasses path parametrization and retains its existing timing after +canonical validation. Explicit configuration overrides the world-based default. +RoboPlan model composition preserves authored acceleration limits and inserts a +temporary global `2.0 rad/s²` fallback where they are absent. Formal per-joint +acceleration overrides will replace this fallback. + +The Viser panel's **Next plan speed** slider provides runtime speed tuning from +`0.05` to `1.0`. Changing it leaves the accepted plan and any active execution +unchanged; press **Plan** again to generate motion at the new scale. For +joint-space planning the value reduces the selected parametrizer's configured +velocity and acceleration scales. For Cartesian planning Viser puts the same +scale into the native planning request before its timestamps are generated. + RoboPlan Cartesian options are supplied per planning request: ```python skip @@ -250,9 +292,11 @@ not need extra setup because it observes the Drake world directly. Previews use the stored synchronized `JointTrajectory` from the generated plan. Viser projects the globally named trajectory into robot-local preview ghosts and plays the stored timestamped points directly; optional preview duration only -scales the stored delays. Execute freshness is enforced by the manipulation -module/operator immediately before dispatch, not by Viser-side telemetry -snapshots. +scales the stored delays. Execution projects that same accepted trajectory into +each robot's local joint order while preserving timestamps and velocities; it +does not regenerate or retime it. Execute freshness is enforced by the +manipulation module/operator immediately before dispatch, not by Viser-side +telemetry snapshots. ### Perception + Agent diff --git a/pyproject.toml b/pyproject.toml index c9bd52c7a1..0331b738dd 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -291,7 +291,7 @@ manipulation = [ # Other "matplotlib>=3.7.1", "pyyaml>=6.0", - "roboplan>=0.5.1", + "roboplan==0.5.1", ] cpu = [ @@ -456,7 +456,7 @@ lint = [ "pytest==8.3.5", "python-can>=4", "python-socketio>=5.16.1", - "roboplan>=0.5.1", + "roboplan==0.5.1", "sounddevice>=0.5.5", "trimesh>=4.12", "watchdog>=3.0.0", diff --git a/uv.lock b/uv.lock index 7f59f0ac18..f2132292f5 100644 --- a/uv.lock +++ b/uv.lock @@ -2118,7 +2118,7 @@ requires-dist = [ { name = "reportlab", marker = "extra == 'apriltag'", specifier = ">=4.5.0" }, { name = "rerun-sdk", specifier = "==0.32.0" }, { name = "rerun-sdk", marker = "extra == 'visualization'", specifier = "==0.32.0" }, - { name = "roboplan", marker = "extra == 'manipulation'", specifier = ">=0.5.1" }, + { name = "roboplan", marker = "extra == 'manipulation'", specifier = "==0.5.1" }, { name = "scipy", specifier = ">=1.15.1" }, { name = "sortedcontainers", specifier = "==2.4.0" }, { name = "sounddevice", marker = "extra == 'agents'" }, @@ -2180,7 +2180,7 @@ lint = [ { name = "pytest", specifier = "==8.3.5" }, { name = "python-can", specifier = ">=4" }, { name = "python-socketio", specifier = ">=5.16.1" }, - { name = "roboplan", specifier = ">=0.5.1" }, + { name = "roboplan", specifier = "==0.5.1" }, { name = "ruff", specifier = "==0.14.3" }, { name = "sounddevice", specifier = ">=0.5.5" }, { name = "tensorboard", specifier = "==2.20.0" },