diff --git a/AGENTS.md b/AGENTS.md index 4da2b37404..b2b83d7d67 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -44,7 +44,7 @@ dimos restart # stop + re-run with same original args | `xarm-perception-sim-agent` | xArm | sim | GPT-4o | — | Manipulation + perception + agent, sim | | `xarm7-planner-coordinator` | xArm7 | real | — | — | Trajectory planner coordinator | | `teleop-quest-xarm7` | xArm7 | real | — | — | Quest VR teleop | -| `dual-xarm6-planner` | xArm6×2 | real | — | — | Dual-arm motion planner | +| `dual-xarm6-planner-coordinator` | xArm6×2 | real | — | — | Dual-arm motion planner + coordinator | Run `dimos list` for the full list. diff --git a/CONTEXT.md b/CONTEXT.md new file mode 100644 index 0000000000..eceab2a274 --- /dev/null +++ b/CONTEXT.md @@ -0,0 +1,89 @@ +# DimOS Robotics Language + +Shared vocabulary for DimOS robotics concepts. These terms define domain language, not implementation details. + +## Language + +**Robot Name**: +A stable planning-domain identity for a concrete robot/model instance, used in public planning group and flat joint-name scoping. +_Avoid_: World robot ID, hardware ID, namespace + +**World Robot ID**: +An internal planning-world handle for a robot after it has been added to a backend world. +_Avoid_: Robot name, hardware ID, joint namespace + +**Hardware ID**: +A control-layer routing identity for a hardware component. For manipulator robots, it normally matches the robot name at the coordinator boundary. +_Avoid_: Robot name when discussing planning semantics, world robot ID + +**Planning Group**: +A named selectable serial kinematic chain of robot joints used as the unit of manipulation planning. After binding to a robot name, it is identified by a planning group ID and remains independent of backend world robot IDs. +_Avoid_: Move group, movegroup + +**Planning Group Definition**: +The model-level declaration of a planning group before it is bound to a concrete robot in a planning world. +_Avoid_: Runtime group, robot ID + +**End-Effector Association**: +Separate metadata used for pose-targeted operations. For a planning group defined by a chain, the end-effector link is the chain tip. For a planning group defined only by joints, there is no end-effector link. +_Avoid_: Planning group definition + +**Planning Group Selection**: +The set of one or more planning groups chosen for a planning request. +_Avoid_: Composite group + +**Planning Target Set**: +The atomic manipulation UI/planning state built on top of a planning group selection: selected planning groups, target authoring state, combined IK joint target, whole-set feasibility, and generated plan. Per-group UI panels are views into this target set, not independent planning states. +_Avoid_: Independent group cards, per-robot plan state + +**Auxiliary Planning Group**: +A planning group selected to participate in a planning target set without receiving a direct end-effector pose target in that request. It is solved, checked, planned, previewed, and executed with the whole target set; it simply has no assigned target gizmo. A planning group may be auxiliary in one request and directly targeted in another. +_Avoid_: Joint-only group, intrinsic auxiliary group + +**Coordinated Planning Problem**: +A planning request over one or more selected planning groups that is solved as one combined joint-space problem with one synchronized result. +_Avoid_: Batch planning, independent planning + +**Planning Group ID**: +An API-level identifier for a planning group, always written as `{robot_name}/{group_name}`. `/` is reserved as the delimiter and is not part of either component. +_Avoid_: Bare group name, robot ID + +**Default Planning Group**: +The generated fallback planning group used by robot-scoped compatibility APIs when no planning group is specified explicitly. It is not inferred from arbitrary SRDF group uniqueness. +_Avoid_: Unique planning group, primary planning group + +**Robot-Scoped Compatibility API**: +A convenience API that accepts a robot name for common single-robot calls but immediately delegates to planning-group APIs through the robot's default planning group. It does not define a separate planning model, storage model, or groupless execution path. +_Avoid_: Robot-scoped planner, groupless API + +**Joint State**: +A joint-name-keyed robot state that can represent any set of joints and is not inherently coupled to a robot, planning group, planning group selection, or joint-name scope. At flat multi-robot or coordinator boundaries, joint names are required and are global joint names. Robot identity and local-vs-global meaning are provided by the API boundary or containing type, not by extra fields on the generic joint state. +_Avoid_: Planning-group-scoped state + +**Robot Model Joint Names**: +The ordered controllable joints of a robot model in the model's local namespace. This usually aligns with the model's actuated joints, but is not itself a planning group. +_Avoid_: Implicit planning group + +**Local Model Joint Name**: +A joint name as it appears inside a robot model or SRDF before the model is bound to a concrete robot in a planning world. +_Avoid_: Runtime joint name, coordinator joint name + +**Robot-Scoped Joint State**: +A single-robot joint state whose robot identity is explicit outside the generic joint state. Robot-scoped APIs may accept unnamed ordered joint vectors in robot model joint order; when joint names are present, they are local model joint names because the robot identity is already explicit. +_Avoid_: Namespaced local joint state, prefixed joint state + +**Generated Plan**: +A flat planning result that may contain one or more robots. Joint states in a generated plan require names and use global joint names so the plan remains unambiguous across robot boundaries. +_Avoid_: Robot-scoped joint plan, local joint plan + +**Group-Scoped Preview**: +A visualization request for a generated plan over a planning group or planning group selection. A visualization backend may render the whole robot when partial-group rendering is not practical, but the API scope remains the generated plan's selected planning groups. +_Avoid_: Robot-scoped preview API + +**Global Joint Name**: +A boundary-level joint name that mechanically combines a robot name and local model joint name as `{robot_name}/{local_joint_name}` so it is stable and unique in flat joint-state representations, even when the local model joint name is already descriptive. `/` is reserved as the delimiter and is not part of either component. +_Avoid_: Resolved joint name, coordinator joint name, bare joint name, local joint name + +**Robot Placement**: +The placement of a robot model within the planning world. Robot placement belongs in the robot model description rather than in a separate planning configuration transform. +_Avoid_: Planning base pose, config placement transform diff --git a/dimos/control/README.md b/dimos/control/README.md index 858af0b6c4..56694a2c38 100644 --- a/dimos/control/README.md +++ b/dimos/control/README.md @@ -27,7 +27,6 @@ Centralized control system for multi-arm robots with per-joint arbitration. ```bash # Terminal 1: Run coordinator dimos run coordinator-mock # Single 7-DOF mock arm -dimos run coordinator-dual-mock # Dual arms (7+6 DOF) dimos run coordinator-piper-xarm # Real hardware # Terminal 2: Control via CLI diff --git a/dimos/control/blueprints/dual.py b/dimos/control/blueprints/dual.py index 07450bbefb..3444f23335 100644 --- a/dimos/control/blueprints/dual.py +++ b/dimos/control/blueprints/dual.py @@ -58,10 +58,16 @@ hardware=[_xarm7_left, _xarm6_right], tasks=[ TaskConfig( - name="traj_left", type="trajectory", joint_names=_xarm7_left.joints, priority=10 + name="traj_left_arm", + type="trajectory", + joint_names=_xarm7_left.joints, + priority=10, ), TaskConfig( - name="traj_right", type="trajectory", joint_names=_xarm6_right.joints, priority=10 + name="traj_right_arm", + type="trajectory", + joint_names=_xarm6_right.joints, + priority=10, ), ], ) @@ -92,3 +98,10 @@ ), ], ) + + +__all__ = [ + "coordinator_dual_mock", + "coordinator_dual_xarm", + "coordinator_piper_xarm", +] diff --git a/dimos/control/blueprints/test_dual.py b/dimos/control/blueprints/test_dual.py new file mode 100644 index 0000000000..a61eca0080 --- /dev/null +++ b/dimos/control/blueprints/test_dual.py @@ -0,0 +1,69 @@ +# 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 dual-arm control blueprints.""" + +from dimos.control.blueprints.dual import coordinator_dual_xarm +from dimos.control.coordinator import ControlCoordinator +from dimos.manipulation.blueprints import dual_xarm6_planner_coordinator +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.manipulation.planning.planning_identifiers import make_global_joint_names + + +def _coordinator_task_names(blueprint) -> list[str]: + atom = next(atom for atom in blueprint.blueprints if atom.module is ControlCoordinator) + return [task.name for task in atom.kwargs["tasks"]] + + +def _coordinator_tasks(blueprint): + atom = next(atom for atom in blueprint.blueprints if atom.module is ControlCoordinator) + return atom.kwargs["tasks"] + + +def _manipulation_robots(blueprint): + atom = next(atom for atom in blueprint.blueprints if atom.module is ManipulationModule) + return atom.kwargs["robots"] + + +def _manipulation_visualization(blueprint): + atom = next(atom for atom in blueprint.blueprints if atom.module is ManipulationModule) + return atom.kwargs["visualization"] + + +def test_dual_xarm6_integrated_blueprint_has_planner_and_coordinator() -> None: + modules = [atom.module for atom in dual_xarm6_planner_coordinator.blueprints] + + assert ManipulationModule in modules + assert ControlCoordinator in modules + + +def test_dual_xarm6_integrated_blueprint_uses_viser_for_execution_ui() -> None: + visualization = _manipulation_visualization(dual_xarm6_planner_coordinator) + + assert visualization == {"backend": "viser", "allow_plan_execute": True} + + +def test_dual_xarm6_integrated_tasks_match_planner_robots() -> None: + tasks_by_name = {task.name: task for task in _coordinator_tasks(dual_xarm6_planner_coordinator)} + + for robot in _manipulation_robots(dual_xarm6_planner_coordinator): + task = tasks_by_name[robot.coordinator_task_name] + assert task.joint_names == make_global_joint_names(robot.name, robot.joint_names) + + +def test_dual_coordinator_xarm_task_names_match_manipulation_robot_defaults() -> None: + assert _coordinator_task_names(coordinator_dual_xarm) == [ + "traj_left_arm", + "traj_right_arm", + ] diff --git a/dimos/control/tasks/trajectory_task/trajectory_task.py b/dimos/control/tasks/trajectory_task/trajectory_task.py index a3eb23dec3..f26ca72ec6 100644 --- a/dimos/control/tasks/trajectory_task/trajectory_task.py +++ b/dimos/control/tasks/trajectory_task/trajectory_task.py @@ -191,6 +191,23 @@ def execute(self, trajectory: JointTrajectory) -> bool: logger.warning(f"Empty trajectory for {self._name}") return False + if trajectory.joint_names and trajectory.joint_names != self._joint_names_list: + logger.warning( + f"Joint name mismatch for {self._name}: " + f"expected={self._joint_names_list}, received={trajectory.joint_names}" + ) + return False + + if not trajectory.joint_names: + expected_joint_count = len(self._joint_names_list) + for point in trajectory.points: + if len(point.positions) != expected_joint_count: + logger.warning( + f"Trajectory point dimension mismatch for {self._name}: " + f"expected={expected_joint_count}, received={len(point.positions)}" + ) + return False + # Preempt any active trajectory if self._state == TrajectoryState.EXECUTING: logger.info(f"Preempting active trajectory on {self._name}") diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index ae6bc1e9de..13f0579cb3 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -274,6 +274,29 @@ def test_execute_trajectory(self, trajectory_task, simple_trajectory): assert trajectory_task.is_active() assert trajectory_task.get_state() == TrajectoryState.EXECUTING + def test_execute_rejects_mismatched_joint_names(self, trajectory_task): + trajectory = JointTrajectory( + joint_names=["other/joint1", "other/joint2", "other/joint3"], + points=[ + TrajectoryPoint( + positions=[0.0, 0.0, 0.0], + velocities=[0.0, 0.0, 0.0], + time_from_start=0.0, + ), + TrajectoryPoint( + positions=[1.0, 0.5, 0.25], + velocities=[0.0, 0.0, 0.0], + time_from_start=1.0, + ), + ], + ) + + result = trajectory_task.execute(trajectory) + + assert result is False + assert not trajectory_task.is_active() + assert trajectory_task.get_state() == TrajectoryState.IDLE + def test_compute_during_trajectory(self, trajectory_task, simple_trajectory, coordinator_state): t_start = time.perf_counter() trajectory_task.execute(simple_trajectory) @@ -530,6 +553,28 @@ def test_tick_loop_calls_compute(self, mock_adapter): assert mock_task.compute.call_count > 0 + def test_write_all_hardware_logs_rejected_command(self, mocker): + hardware = {"arm": MagicMock()} + hardware["arm"].write_command.return_value = False + log_error = mocker.patch("dimos.control.tick_loop.logger.error") + tick_loop = TickLoop( + tick_rate=100.0, + hardware=hardware, + hardware_lock=threading.Lock(), + tasks={}, + task_lock=threading.Lock(), + joint_to_hardware={}, + ) + + tick_loop._write_all_hardware({"arm": ({"arm/joint1": 0.5}, ControlMode.SERVO_POSITION)}) + + hardware["arm"].write_command.assert_called_once_with( + {"arm/joint1": 0.5}, ControlMode.SERVO_POSITION + ) + log_error.assert_called_once_with( + "Hardware %s rejected %d %s command(s)", "arm", 1, "SERVO_POSITION" + ) + class TestIntegration: def test_full_trajectory_execution(self, mock_adapter): diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index 975dfa9333..6184591116 100644 --- a/dimos/control/tick_loop.py +++ b/dimos/control/tick_loop.py @@ -397,7 +397,13 @@ def _write_all_hardware( for hw_id, (positions, mode) in hw_commands.items(): if hw_id in self._hardware: try: - self._hardware[hw_id].write_command(positions, mode) + if not self._hardware[hw_id].write_command(positions, mode): + logger.error( + "Hardware %s rejected %d %s command(s)", + hw_id, + len(positions), + mode.name, + ) except Exception as e: logger.error(f"Failed to write to {hw_id}: {e}") diff --git a/dimos/e2e_tests/test_control_coordinator.py b/dimos/e2e_tests/test_control_coordinator.py index 40e4800b47..8d635805b8 100644 --- a/dimos/e2e_tests/test_control_coordinator.py +++ b/dimos/e2e_tests/test_control_coordinator.py @@ -201,8 +201,8 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: """Test dual-arm coordinator with independent trajectories.""" lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs.JointState") - # Start dual-arm mock coordinator - start_blueprint("coordinator-dual-mock") + # Start integrated dual-arm mock planner/coordinator + start_blueprint("dual-xarm6-planner-coordinator") lcm_spy.wait_for_saved_topic("/coordinator_joint_state#sensor_msgs.JointState") client = RPCClient(None, ControlCoordinator) @@ -213,15 +213,15 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: assert "right_arm/joint1" in joints tasks = client.list_tasks() - assert "traj_left" in tasks - assert "traj_right" in tasks + assert "traj_left_arm" in tasks + assert "traj_right_arm" in tasks # Create trajectories for both arms left_trajectory = JointTrajectory( - joint_names=[f"left_arm/joint{i + 1}" for i in range(7)], + joint_names=[f"left_arm/joint{i + 1}" for i in range(6)], points=[ - TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 7), - TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 7), + TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 6), + TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 6), ], ) @@ -235,10 +235,11 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: # Execute both via task_invoke assert ( - client.task_invoke("traj_left", "execute", {"trajectory": left_trajectory}) is True + client.task_invoke("traj_left_arm", "execute", {"trajectory": left_trajectory}) + is True ) assert ( - client.task_invoke("traj_right", "execute", {"trajectory": right_trajectory}) + client.task_invoke("traj_right_arm", "execute", {"trajectory": right_trajectory}) is True ) @@ -246,8 +247,8 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None: time.sleep(1.0) # Both should complete - left_state = client.task_invoke("traj_left", "get_state") - right_state = client.task_invoke("traj_right", "get_state") + left_state = client.task_invoke("traj_left_arm", "get_state") + right_state = client.task_invoke("traj_right_arm", "get_state") assert left_state == TrajectoryState.COMPLETED assert right_state == TrajectoryState.COMPLETED diff --git a/dimos/e2e_tests/test_manipulation_planning_groups.py b/dimos/e2e_tests/test_manipulation_planning_groups.py new file mode 100644 index 0000000000..24b526e24e --- /dev/null +++ b/dimos/e2e_tests/test_manipulation_planning_groups.py @@ -0,0 +1,212 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Large E2E tests for manipulation planning groups with a coordinator. + +These tests launch a real ManipulationModule + ControlCoordinator blueprint and +exercise the public planning RPCs over LCM, matching the self-hosted large-test +style used by the navigation stack. +""" + +from __future__ import annotations + +from collections.abc import Callable +import time +from typing import Any + +import pytest + +from dimos.control.coordinator import ControlCoordinator +from dimos.core.rpc_client import RPCClient +from dimos.e2e_tests.dimos_cli_call import DimosCliCall +from dimos.e2e_tests.lcm_spy import LcmSpy +from dimos.manipulation.manipulation_module import ManipulationModule +from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.msgs.trajectory_msgs.TrajectoryStatus import TrajectoryState + +pytestmark = [pytest.mark.self_hosted_large] + +JOINT_STATE_TOPIC = "/coordinator/joint_state#sensor_msgs.JointState" +BLUEPRINT = "openarm-mock-planner-coordinator" + + +def _wait_for_robot_info( + client: RPCClient, + robot_name: str, + *, + timeout: float = 120.0, +) -> dict[str, Any]: + deadline = time.time() + timeout + last_error: BaseException | None = None + while time.time() < deadline: + try: + info = client.get_robot_info(robot_name) + if info and info.get("planning_groups"): + return info + except BaseException as exc: + last_error = exc + time.sleep(0.5) + raise TimeoutError(f"Timed out waiting for {robot_name!r} robot info") from last_error + + +def _wait_for_trajectory_completion( + client: RPCClient, + robot_name: str, + *, + timeout: float = 10.0, +) -> None: + deadline = time.time() + timeout + last_status: dict[str, Any] | None = None + while time.time() < deadline: + last_status = client.get_trajectory_status(robot_name) + if last_status is not None and last_status.get("state") == TrajectoryState.COMPLETED: + return + time.sleep(0.1) + raise TimeoutError(f"{robot_name!r} trajectory did not complete; last={last_status}") + + +def _wait_for_manipulation_state( + client: RPCClient, + state_name: str, + *, + timeout: float = 10.0, +) -> None: + deadline = time.time() + timeout + last_state: str | None = None + while time.time() < deadline: + last_state = client.get_state() + if last_state == state_name: + return + time.sleep(0.1) + raise TimeoutError(f"ManipulationModule did not reach {state_name}; last={last_state}") + + +def _wait_for_current_joints( + client: RPCClient, + robot_names: tuple[str, ...], + *, + timeout: float = 10.0, +) -> None: + deadline = time.time() + timeout + missing = robot_names + while time.time() < deadline: + missing = tuple( + robot_name + for robot_name in robot_names + if client.get_current_joints(robot_name) is None + ) + if not missing: + return + time.sleep(0.1) + raise TimeoutError(f"Timed out waiting for current joints from {missing}") + + +def _prepare_for_planning(client: RPCClient, robot_names: tuple[str, ...]) -> None: + client.reset() + _wait_for_manipulation_state(client, "IDLE") + _wait_for_current_joints(client, robot_names) + # Robot info and joint-state topics can become available just before the + # manipulation module finishes finalizing world monitors. Require a stable + # ready state after joint state is flowing to avoid command-readiness flakes. + time.sleep(0.25) + _wait_for_manipulation_state(client, "IDLE") + + +def _planning_group_id(info: dict[str, Any]) -> str: + groups = info["planning_groups"] + assert len(groups) == 1 + group_id = groups[0]["id"] + assert isinstance(group_id, str) + return group_id + + +def _offset_target(client: RPCClient, robot_name: str, delta: float) -> JointState: + current = client.get_current_joints(robot_name) + assert current is not None + return JointState(position=[position + delta for position in current]) + + +def _start_openarm_mock_planner( + start_blueprint: Callable[..., DimosCliCall], lcm_spy: LcmSpy +) -> None: + lcm_spy.save_topic(JOINT_STATE_TOPIC) + start_blueprint(BLUEPRINT) + lcm_spy.wait_for_saved_topic(JOINT_STATE_TOPIC, timeout=120.0) + + +def test_single_arm_plans_and_executes_through_control_coordinator( + lcm_spy: LcmSpy, + start_blueprint: Callable[..., DimosCliCall], +) -> None: + """Plan with one arm and execute through its trajectory task.""" + _start_openarm_mock_planner(start_blueprint, lcm_spy) + + client = RPCClient(None, ManipulationModule) + coordinator_client = RPCClient(None, ControlCoordinator) + try: + left_info = _wait_for_robot_info(client, "left_arm") + left_id = _planning_group_id(left_info) + + tasks = coordinator_client.list_tasks() + assert left_info["coordinator_task_name"] in tasks + + _prepare_for_planning(client, ("left_arm",)) + + planned = client.plan_to_joint_targets({left_id: _offset_target(client, "left_arm", 0.02)}) + assert planned, client.get_error() + assert client.has_planned_path() + assert client.execute_plan(robot_name="left_arm") + + _wait_for_trajectory_completion(client, "left_arm") + finally: + coordinator_client.stop_rpc_client() + client.stop_rpc_client() + + +def test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator( + lcm_spy: LcmSpy, + start_blueprint: Callable[..., DimosCliCall], +) -> None: + """Plan one generated plan over both arms and dispatch both JTC tasks.""" + _start_openarm_mock_planner(start_blueprint, lcm_spy) + + client = RPCClient(None, ManipulationModule) + coordinator_client = RPCClient(None, ControlCoordinator) + try: + left_info = _wait_for_robot_info(client, "left_arm") + right_info = _wait_for_robot_info(client, "right_arm") + left_id = _planning_group_id(left_info) + right_id = _planning_group_id(right_info) + + tasks = coordinator_client.list_tasks() + assert left_info["coordinator_task_name"] in tasks + assert right_info["coordinator_task_name"] in tasks + + _prepare_for_planning(client, ("left_arm", "right_arm")) + + planned = client.plan_to_joint_targets( + { + left_id: _offset_target(client, "left_arm", 0.02), + right_id: _offset_target(client, "right_arm", -0.02), + } + ) + assert planned, client.get_error() + assert client.has_planned_path() + assert client.execute_plan() + + _wait_for_trajectory_completion(client, "left_arm") + _wait_for_trajectory_completion(client, "right_arm") + finally: + coordinator_client.stop_rpc_client() + client.stop_rpc_client() diff --git a/dimos/manipulation/blueprints.py b/dimos/manipulation/blueprints.py index 5207135638..6c4b2764d3 100644 --- a/dimos/manipulation/blueprints.py +++ b/dimos/manipulation/blueprints.py @@ -25,165 +25,99 @@ # 3. Interactive RPC client (plan, preview, execute from Python): dimos run xarm7-planner-coordinator python -i -m dimos.manipulation.planning.examples.manipulation_client + + # 4. Dual-arm xArm7 mock planner + coordinator demo: + dimos run dual-xarm7-planner-coordinator + python -i -m dimos.manipulation.planning.examples.manipulation_client """ import math -from pathlib import Path -from typing import Any from dimos.agents.mcp.mcp_client import McpClient from dimos.agents.mcp.mcp_server import McpServer -from dimos.control.blueprints._hardware import XARM7_SIM_PATH, manipulator -from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config +from dimos.core.transport import LCMTransport from dimos.hardware.sensors.camera.realsense.camera import RealSenseCamera from dimos.manipulation.manipulation_module import ManipulationModule from dimos.manipulation.pick_and_place_module import PickAndPlaceModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.JointState import JointState from dimos.perception.object_scene_registration import ObjectSceneRegistrationModule -from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule -from dimos.utils.data import LfsPath -from dimos.visualization.rerun.bridge import RerunBridgeModule - -XARM_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("right_inner_knuckle", "right_outer_knuckle"), - ("left_inner_knuckle", "left_outer_knuckle"), - ("right_inner_knuckle", "right_finger"), - ("left_inner_knuckle", "left_finger"), - ("left_finger", "right_finger"), - ("left_outer_knuckle", "right_outer_knuckle"), - ("left_inner_knuckle", "right_inner_knuckle"), - ("left_outer_knuckle", "right_finger"), - ("right_outer_knuckle", "left_finger"), - ("xarm_gripper_base_link", "left_inner_knuckle"), - ("xarm_gripper_base_link", "right_inner_knuckle"), - ("xarm_gripper_base_link", "left_finger"), - ("xarm_gripper_base_link", "right_finger"), - ("link6", "xarm_gripper_base_link"), - ("link6", "left_outer_knuckle"), - ("link6", "right_outer_knuckle"), -] - -_XARM_MODEL_PATH = LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro" -_XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")} - - -def _base_pose(x: float = 0.0, y: float = 0.0, z: float = 0.0) -> PoseStamped: - return PoseStamped( - position=Vector3(x=x, y=y, z=z), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ) - - -def _coordinator_joint_mapping( - name: str, - dof: int, - *, - joint_prefix: str | None = None, -) -> dict[str, str]: - prefix = f"{name}/" if joint_prefix is None else joint_prefix - if not prefix: - return {} - return {f"{prefix}joint{i}": f"joint{i}" for i in range(1, dof + 1)} - - -def _make_xarm_model_config( - name: str, - dof: int, - *, - add_gripper: bool = True, - x_offset: float = 0.0, - y_offset: float = 0.0, - z_offset: float = 0.0, - pitch: float = 0.0, - joint_prefix: str | None = None, - coordinator_task_name: str | None = None, - tf_extra_links: list[str] | None = None, - home_joints: list[float] | None = None, - pre_grasp_offset: float = 0.10, -) -> RobotModelConfig: - xacro_args = { - "dof": str(dof), - "limited": "true", - "attach_xyz": f"{x_offset} {y_offset} {z_offset}", - "attach_rpy": f"0 {pitch} 0", - } - if add_gripper: - xacro_args["add_gripper"] = "true" - - return RobotModelConfig( - name=name, - model_path=_XARM_MODEL_PATH, - base_pose=_base_pose(x_offset, y_offset, z_offset), - joint_names=[f"joint{i}" for i in range(1, dof + 1)], - end_effector_link="link_tcp" if add_gripper else f"link{dof}", - base_link="link_base", - package_paths=_XARM_PACKAGE_PATHS, - xacro_args=xacro_args, - auto_convert_meshes=True, - collision_exclusion_pairs=(XARM_GRIPPER_COLLISION_EXCLUSIONS if add_gripper else []), - joint_name_mapping=_coordinator_joint_mapping( - name, - dof, - joint_prefix=joint_prefix, - ), - coordinator_task_name=coordinator_task_name or f"traj_{name}", - gripper_hardware_id=name if add_gripper else None, - tf_extra_links=tf_extra_links or [], - home_joints=home_joints or [0.0] * dof, - pre_grasp_offset=pre_grasp_offset, - ) - - -def _make_xarm6_model_config( - name: str = "arm", - **kwargs: Any, -) -> RobotModelConfig: - return _make_xarm_model_config(name, 6, **kwargs) - - -def _make_xarm7_model_config( - name: str = "arm", - **kwargs: Any, -) -> RobotModelConfig: - return _make_xarm_model_config(name, 7, **kwargs) +from dimos.robot.catalog.ufactory import xarm6 as _catalog_xarm6, xarm7 as _catalog_xarm7 +# Single XArm6 planner (standalone, no coordinator) +_xarm6_planner_cfg = _catalog_xarm6( + name="arm", + adapter_type="xarm" if global_config.xarm6_ip else "mock", + address=global_config.xarm6_ip, +) xarm6_planner_only = ManipulationModule.blueprint( - robots=[_make_xarm6_model_config(name="arm")], + robots=[_xarm6_planner_cfg.to_robot_model_config()], planning_timeout=10.0, visualization={"backend": "meshcat"}, ) -dual_xarm6_planner = ManipulationModule.blueprint( - robots=[ - _make_xarm6_model_config(name="left_arm", y_offset=0.5), - _make_xarm6_model_config(name="right_arm", y_offset=-0.5), - ], - planning_timeout=10.0, - visualization={"backend": "meshcat"}, +# Dual XArm6 planner + coordinator with Viser execution UI. +_left_arm_cfg = _catalog_xarm6( + name="left_arm", + adapter_type="xarm" if global_config.xarm6_ip else "mock", + address=global_config.xarm6_ip, + y_offset=0.5, +) +_right_arm_cfg = _catalog_xarm6( + name="right_arm", + adapter_type="xarm" if global_config.xarm6_ip else "mock", + address=global_config.xarm6_ip, + y_offset=-0.5, +) + +dual_xarm6_planner_coordinator = autoconnect( + ManipulationModule.blueprint( + robots=[ + _left_arm_cfg.to_robot_model_config(), + _right_arm_cfg.to_robot_model_config(), + ], + planning_timeout=10.0, + visualization={"backend": "viser", "allow_plan_execute": True}, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + _left_arm_cfg.to_hardware_component(), + _right_arm_cfg.to_hardware_component(), + ], + tasks=[ + _left_arm_cfg.to_task_config(), + _right_arm_cfg.to_task_config(), + ], + ), +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } ) # Single XArm7 planner + coordinator (uses real hardware when XARM7_IP is set) # Usage: XARM7_IP= dimos run xarm7-planner-coordinator -_xarm7_hw = manipulator( - "arm", - 7, +_xarm7_cfg = _catalog_xarm7( + name="arm", adapter_type="xarm" if global_config.xarm7_ip else "mock", address=global_config.xarm7_ip, - gripper=True, + add_gripper=True, ) xarm7_planner_coordinator = autoconnect( ManipulationModule.blueprint( - robots=[_make_xarm7_model_config(name="arm", add_gripper=True)], + robots=[_xarm7_cfg.to_robot_model_config()], planning_timeout=10.0, visualization={"backend": "meshcat"}, ), @@ -191,16 +125,53 @@ def _make_xarm7_model_config( tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[_xarm7_hw], + hardware=[_xarm7_cfg.to_hardware_component()], + tasks=[_xarm7_cfg.to_task_config()], + ), +) + + +# Dual XArm7 mock planner + coordinator for bimanual planning demos. +# Usage: dimos run dual-xarm7-planner-coordinator +_left_xarm7_cfg = _catalog_xarm7( + name="left_arm", + adapter_type="mock", + add_gripper=False, + y_offset=0.5, +) +_right_xarm7_cfg = _catalog_xarm7( + name="right_arm", + adapter_type="mock", + add_gripper=False, + y_offset=-0.5, +) + +dual_xarm7_planner_coordinator = autoconnect( + ManipulationModule.blueprint( + robots=[ + _left_xarm7_cfg.to_robot_model_config(), + _right_xarm7_cfg.to_robot_model_config(), + ], + planning_timeout=10.0, + visualization={"backend": "viser", "allow_plan_execute": True}, + ), + ControlCoordinator.blueprint( + tick_rate=100.0, + publish_joint_state=True, + joint_state_frame_id="coordinator", + hardware=[ + _left_xarm7_cfg.to_hardware_component(), + _right_xarm7_cfg.to_hardware_component(), + ], tasks=[ - TaskConfig( - name="traj_arm", - type="trajectory", - joint_names=_xarm7_hw.joints, - priority=10, - ), + _left_xarm7_cfg.to_task_config(), + _right_xarm7_cfg.to_task_config(), ], ), +).transports( + { + ("joint_state", JointState): LCMTransport("/coordinator/joint_state", JointState), + } ) @@ -250,16 +221,18 @@ def _make_xarm7_model_config( rotation=Quaternion(0.70513398, 0.00535696, 0.70897578, -0.01052180), # xyzw ) +_xarm7_perception_cfg = _catalog_xarm7( + name="arm", + adapter_type="xarm" if global_config.xarm7_ip else "mock", + address=global_config.xarm7_ip, + pitch=math.radians(45), + add_gripper=True, + tf_extra_links=["link7"], +) + xarm_perception = autoconnect( PickAndPlaceModule.blueprint( - robots=[ - _make_xarm7_model_config( - name="arm", - add_gripper=True, - pitch=math.radians(45), - tf_extra_links=["link7"], - ) - ], + robots=[_xarm7_perception_cfg.to_robot_model_config()], planning_timeout=10.0, visualization={"backend": "meshcat"}, floor_z=-0.02, @@ -358,28 +331,25 @@ def _make_xarm7_model_config( # Sim perception: MujocoSimModule owns the MujocoEngine and publishes both # camera streams and joint state via shared memory. # ShmMujocoAdapter attaches to the same SHM buffers by MJCF path. -_xarm7_sim_home = [0.0, 0.0, 0.0, 0.0, 0.0, -0.7, 0.0] -_xarm7_sim_hw = manipulator( - "arm", - 7, + +from dimos.robot.catalog.ufactory import XARM7_SIM_PATH +from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule +from dimos.visualization.rerun.bridge import RerunBridgeModule + +_xarm7_sim_cfg = _catalog_xarm7( + name="arm", adapter_type="sim_mujoco", address=str(XARM7_SIM_PATH), - gripper=True, - home_joints=_xarm7_sim_home, + add_gripper=True, + pitch=math.radians(45), + tf_extra_links=["link7"], + home_joints=[0.0, 0.0, 0.0, 0.0, 0.0, -0.7, 0.0], + pre_grasp_offset=0.05, ) xarm_perception_sim = autoconnect( PickAndPlaceModule.blueprint( - robots=[ - _make_xarm7_model_config( - name="arm", - add_gripper=True, - pitch=math.radians(45), - tf_extra_links=["link7"], - home_joints=_xarm7_sim_home, - pre_grasp_offset=0.05, - ) - ], + robots=[_xarm7_sim_cfg.to_robot_model_config()], planning_timeout=10.0, visualization={"backend": "meshcat"}, ), @@ -395,15 +365,8 @@ def _make_xarm7_model_config( tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[_xarm7_sim_hw], - tasks=[ - TaskConfig( - name="traj_arm", - type="trajectory", - joint_names=_xarm7_sim_hw.joints, - priority=10, - ), - ], + hardware=[_xarm7_sim_cfg.to_hardware_component()], + tasks=[_xarm7_sim_cfg.to_task_config()], ), RerunBridgeModule.blueprint(), ) @@ -414,3 +377,16 @@ def _make_xarm7_model_config( McpServer.blueprint(), McpClient.blueprint(system_prompt=_MANIPULATION_AGENT_SYSTEM_PROMPT), ) + + +__all__ = [ + "dual_xarm6_planner_coordinator", + "dual_xarm7_planner_coordinator", + "xarm6_planner_only", + "xarm7_planner_coordinator", + "xarm7_planner_coordinator_agent", + "xarm_perception", + "xarm_perception_agent", + "xarm_perception_sim", + "xarm_perception_sim_agent", +] diff --git a/dimos/manipulation/control/coordinator_client.py b/dimos/manipulation/control/coordinator_client.py index ed552e0846..6fe78d13cc 100644 --- a/dimos/manipulation/control/coordinator_client.py +++ b/dimos/manipulation/control/coordinator_client.py @@ -24,12 +24,10 @@ Usage: # Terminal 1: Start the coordinator dimos run coordinator-mock # Single arm - dimos run coordinator-dual-mock # Dual arm # Terminal 2: Run this client python -m dimos.manipulation.control.coordinator_client - python -m dimos.manipulation.control.coordinator_client --task traj_left - python -m dimos.manipulation.control.coordinator_client --task traj_right + python -m dimos.manipulation.control.coordinator_client --task traj_arm How it works: 1. Connects to ControlCoordinator via LCM RPC diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index 0d4dfff226..fc21073f82 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -15,7 +15,7 @@ """Manipulation Module - Motion planning with ControlCoordinator execution. Base module providing core manipulation infrastructure: -- @rpc: Low-level building blocks (plan_to_pose, plan_to_joints, preview_path, execute) +- @rpc: Low-level building blocks (plan_to_pose, plan_to_joints, preview_plan, execute) - @skill (short-horizon): Single-step actions (move_to_pose, open_gripper, go_home, go_init) Subclass PickAndPlaceModule (pick_and_place_module.py) adds perception integration @@ -24,12 +24,13 @@ from __future__ import annotations +from collections.abc import Mapping, Sequence from enum import Enum import threading import time +import traceback from typing import TYPE_CHECKING, Any, TypeAlias -import numpy as np from pydantic import Field from dimos.agents.annotation import skill @@ -39,17 +40,29 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In from dimos.manipulation.planning.factory import create_planning_specs, create_world +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.groups.utils import ( + joint_target_to_global_names, + planning_group_id_from_selector, +) from dimos.manipulation.planning.kinematics.config import ( ManipulationKinematicsConfig, PinkKinematicsConfig, ) from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor +from dimos.manipulation.planning.planning_identifiers import ( + assert_global_joint_names, + assert_local_joint_names, + make_global_joint_names, +) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus, ObstacleType from dimos.manipulation.planning.spec.models import ( + GeneratedPlan, IKResult, - JointPath, Obstacle, + PlanningGroupID, + PlanningResult, RobotName, WorldRobotID, ) @@ -63,9 +76,11 @@ NoManipulationVisualizationConfig, ) from dimos.manipulation.visualization.factory import create_manipulation_visualization -from dimos.manipulation.visualization.types import TargetEvaluation +from dimos.manipulation.visualization.types import TargetEvaluation, TargetSetEvaluation from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.JointState import JointState from dimos.msgs.trajectory_msgs.JointTrajectory import JointTrajectory @@ -83,12 +98,6 @@ RobotRegistry: TypeAlias = dict[RobotName, RobotEntry] """Maps robot_name -> RobotEntry""" -PlannedPaths: TypeAlias = dict[RobotName, JointPath] -"""Maps robot_name -> planned joint path""" - -PlannedTrajectories: TypeAlias = dict[RobotName, JointTrajectory] -"""Maps robot_name -> planned trajectory""" - class ManipulationState(Enum): """State machine for manipulation module.""" @@ -116,6 +125,7 @@ class ManipulationModuleConfig(ModuleConfig): # to prevent the planner from routing trajectories below this height. # Set to None to disable. floor_z: float | None = None + coordinator_rpc_timeout: float = 3.0 class ManipulationModule(Module): @@ -149,9 +159,8 @@ def __init__(self, **kwargs: Any) -> None: # Robot registry: maps robot_name -> (world_robot_id, config, trajectory_gen) self._robots: RobotRegistry = {} - # Stored path for plan/preview/execute workflow (per robot) - self._planned_paths: PlannedPaths = {} - self._planned_trajectories: PlannedTrajectories = {} + # Stored generated plan for preview/execute workflow. + self._last_plan: GeneratedPlan | None = None # Coordinator integration (lazy initialized) self._coordinator_client: RPCClient | None = None @@ -285,27 +294,31 @@ def _get_robot( def _on_joint_state(self, msg: JointState) -> None: """Callback when joint state received from driver. - Splits the aggregated JointState by robot using each robot's - coordinator joint names, then routes to the correct monitor. + Splits the aggregated global JointState by robot, then routes local + robot-scoped states to the correct monitor. """ try: if self._world_monitor is None: return + assert_global_joint_names(msg.name) + # 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(): - coord_names = config.get_coordinator_joint_names() - indices = [name_to_idx.get(cn) for cn in coord_names] + global_names = make_global_joint_names(robot_name, config.joint_names) + indices = [name_to_idx.get(global_name) for global_name in global_names] if any(idx is None for idx in indices): missing = [ - cn for cn, idx in zip(coord_names, indices, strict=False) if idx is None + name + for name, idx in zip(global_names, indices, strict=False) + if idx is None ] logger.warning(f"Skipping '{robot_name}': missing joints {missing}") continue - # Build per-robot sub-message (coordinator namespace) + # Build per-robot sub-message (local model namespace) sub_positions = [msg.position[idx] for idx in indices] # type: ignore[index] sub_velocities = ( [msg.velocity[idx] for idx in indices] # type: ignore[index] @@ -313,7 +326,7 @@ def _on_joint_state(self, msg: JointState) -> None: else [] ) sub_msg = JointState( - name=list(coord_names), + name=list(config.joint_names), position=sub_positions, velocity=sub_velocities, ) @@ -331,14 +344,10 @@ def _on_joint_state(self, msg: JointState) -> None: except Exception as e: logger.error(f"Exception in _on_joint_state: {e}") - import traceback - logger.error(traceback.format_exc()) def _tf_publish_loop(self) -> None: """Publish TF transforms at 10Hz for EE and extra links.""" - from dimos.msgs.geometry_msgs.Transform import Transform - period = 0.1 # 10Hz while not self._tf_stop_event.is_set(): try: @@ -346,10 +355,25 @@ def _tf_publish_loop(self) -> None: break transforms: list[Transform] = [] 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: - ee_tf = Transform.from_pose(config.end_effector_link, ee_pose) + # Publish world → primary planning-group target frame. + # Fall back to robot-scoped EE only for compatibility configs. + # TODO: Publish one TF per pose-targetable group, or expose the + # backend's full robot TF tree, once consumers stop assuming a + # single robot-scoped end-effector frame. + target_frame = config.end_effector_link + pose_group_id = self._primary_pose_group_id_for_robot(config.name) + if pose_group_id is not None: + pose_group = self._world_monitor.planning_groups.get(pose_group_id) + target_frame = pose_group.tip_link + ee_pose = self._world_monitor.get_group_pose(pose_group_id) + else: + ee_pose = ( + self._world_monitor.get_link_pose(robot_id, target_frame) + if target_frame is not None + else None + ) + if ee_pose is not None and target_frame is not None: + ee_tf = Transform.from_pose(target_frame, ee_pose) ee_tf.frame_id = "world" transforms.append(ee_tf) @@ -436,7 +460,13 @@ def get_ee_pose(self, robot_name: RobotName | None = None) -> Pose | None: robot_name: Robot to query (required if multiple robots configured) """ if (robot := self._get_robot(robot_name)) and self._world_monitor: - return self._world_monitor.get_ee_pose(robot[1], joint_state=None) + _, robot_id, config, _ = robot + pose_group_id = self._primary_pose_group_id_for_robot(config.name) + if pose_group_id is not None: + return self._world_monitor.get_group_pose(pose_group_id, joint_state=None) + if config.end_effector_link is None: + return None + return self._world_monitor.get_link_pose(robot_id, config.end_effector_link) return None @rpc @@ -454,25 +484,19 @@ def is_collision_free(self, joints: list[float], robot_name: RobotName | None = return False def _begin_planning( - self, robot_name: RobotName | None = None - ) -> tuple[RobotName, WorldRobotID] | None: - """Check state and begin planning. Returns (robot_name, robot_id) or None. - - Args: - robot_name: Robot to plan for (required if multiple robots configured) - """ + self, group_ids: Sequence[PlanningGroupID] + ) -> tuple[PlanningGroupID, ...] | None: + """Check state and begin planning for the selected planning groups.""" if self._world_monitor is None: logger.error("Planning not initialized") return None - if (robot := self._get_robot(robot_name)) is None: - return None with self._lock: if self._state not in (ManipulationState.IDLE, ManipulationState.COMPLETED): logger.warning(f"Cannot plan: state is {self._state.name}") return None self._planning_epoch += 1 self._state = ManipulationState.PLANNING - return robot[0], robot[1] + return tuple(group_ids) def _fail(self, msg: str) -> bool: """Set FAULT state with error message.""" @@ -481,11 +505,148 @@ def _fail(self, msg: str) -> bool: self._error_message = msg return False - def _dismiss_preview(self, robot_id: WorldRobotID) -> None: + def _default_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: + """Return the generated fallback group used by robot-scoped wrappers.""" + assert self._world_monitor is not None + group_id = self._world_monitor.planning_groups.default_group_id_for_robot(robot_name) + if group_id is not None: + return group_id + logger.error( + "Robot '%s' has no generated default planning group; use explicit group APIs", + robot_name, + ) + return None + + def _primary_pose_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: + """Return the first pose-targetable group for robot-scoped compatibility paths.""" + assert self._world_monitor is not None + return self._world_monitor.planning_groups.primary_pose_group_id_for_robot(robot_name) + + def _current_positions_by_name( + self, robot_name: RobotName, current: JointState + ) -> dict[str, float] | None: + """Index a robot current state by local model joint name.""" + if len(current.name) != len(current.position): + logger.error( + "Current state for '%s' has %d names but %d positions", + robot_name, + len(current.name), + len(current.position), + ) + return None + if not current.name: + logger.error("Current state for '%s' has no joint names", robot_name) + return None + try: + assert_local_joint_names(current.name) + except ValueError as exc: + logger.error("Invalid current state for '%s': %s", robot_name, exc) + return None + + return dict(zip(current.name, current.position, strict=True)) + + def _selected_joint_state(self, group_ids: tuple[PlanningGroupID, ...]) -> JointState | None: + """Collect current state for exactly the selected global joints.""" + assert self._world_monitor is not None + selection = self._world_monitor.planning_groups.select(group_ids) + + robot_ids_by_name: dict[RobotName, WorldRobotID] = {} + for robot_name in selection.robot_names: + try: + robot_ids_by_name[robot_name] = self._robots[robot_name][0] + except KeyError: + logger.error("Robot '%s' is not registered", robot_name) + return None + + current_by_robot: dict[RobotName, dict[str, float]] = {} + for robot_name, robot_id in robot_ids_by_name.items(): + current = self._world_monitor.get_current_joint_state(robot_id) + if current is None: + logger.error("No joint state for robot '%s'", robot_name) + return None + indexed_current = self._current_positions_by_name(robot_name, current) + if indexed_current is None: + return None + current_by_robot[robot_name] = indexed_current + + names: list[str] = [] + positions: list[float] = [] + for group in selection.groups: + robot_state = current_by_robot[group.robot_name] + for resolved_name, local_name in zip( + group.joint_names, group.local_joint_names, strict=True + ): + if local_name not in robot_state: + logger.error("Current state missing selected joint '%s'", resolved_name) + return None + position = robot_state[local_name] + names.append(resolved_name) + positions.append(position) + + return JointState(name=names, position=positions) + + def _joint_target_to_global_names( + self, group_id: PlanningGroupID, target: JointState + ) -> JointState | None: + """Convert a group joint target to global joint names in group order.""" + assert self._world_monitor is not None + group = self._world_monitor.planning_groups.get(group_id) + try: + return joint_target_to_global_names(group, target) + except ValueError as exc: + logger.error(str(exc)) + return None + + def _affected_robot_names(self, plan: GeneratedPlan) -> list[RobotName]: + """Get stable robot names affected by a generated plan.""" + assert self._world_monitor is not None + return list(self._world_monitor.planning_groups.select(plan.group_ids).robot_names) + + def _store_generated_plan( + self, group_ids: tuple[PlanningGroupID, ...], result: PlanningResult + ) -> None: + """Store the canonical generated plan.""" + self._last_plan = GeneratedPlan( + group_ids=group_ids, + path=result.path, + status=result.status, + planning_time=result.planning_time, + path_length=result.path_length, + iterations=result.iterations, + message=result.message, + ) + + def _plan_selected_path( + self, group_ids: tuple[PlanningGroupID, ...], start: JointState, goal: JointState + ) -> bool: + """Plan over an explicit planning group selection and store the result.""" + assert self._world_monitor and self._planner + result = self._planner.plan_selected_joint_path( + world=self._world_monitor.world, + selection=self._world_monitor.planning_groups.select(group_ids), + start=start, + goal=goal, + timeout=self.config.planning_timeout, + ) + if not result.is_success(): + return self._fail(f"Planning failed: {result.status.name}") + + path_joints = list(result.path[-1].name) if result.path else [] + logger.info( + "Path: %d waypoints, groups=%s, joints=%s", + len(result.path), + group_ids, + path_joints, + ) + self._store_generated_plan(group_ids, result) + self._state = ManipulationState.COMPLETED + return True + + def _dismiss_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: """Hide the preview ghost if the world supports it.""" if self._world_monitor is None: return - self._world_monitor.hide_preview(robot_id) + self._world_monitor.hide_preview(group_ids) self._world_monitor.publish_visualization() def _solve_ik_for_pose( @@ -498,9 +659,6 @@ def _solve_ik_for_pose( """Run the configured kinematics backend for a world-frame pose.""" assert self._world_monitor and self._kinematics - # Convert Pose to PoseStamped for the IK solver - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - target_pose = PoseStamped( frame_id="world", position=pose.position, @@ -559,146 +717,147 @@ def solve_ik( @rpc def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: - """Plan motion to pose. Use preview_path() then execute(). + """Plan motion to pose. Use preview_plan() then execute(). Args: pose: Target end-effector pose robot_name: Robot to plan for (required if multiple robots configured) """ - if self._kinematics is None or (r := self._begin_planning(robot_name)) is None: + if self._kinematics is None or self._world_monitor is None: + return False + robot = self._get_robot(robot_name) + if robot is None: + return False + selected_robot_name, _, _, _ = robot + group_id = self._default_group_id_for_robot(selected_robot_name) + if group_id is None: return False - robot_name, robot_id = r - planning_epoch = self._planning_epoch - assert self._world_monitor # guaranteed by _begin_planning + return self.plan_to_poses({group_id: pose}) - current = self._world_monitor.get_current_joint_state(robot_id) - if current is None: + @rpc + def plan_to_poses( + self, + pose_targets: Mapping[PlanningGroupID | PlanningGroup, Pose], + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroup] = (), + ) -> bool: + """Plan to one or more group pose targets with optional auxiliary groups.""" + if self._world_monitor is None or self._kinematics is None: + return False + if not pose_targets: + return self._fail("At least one pose target is required") + + stamped_targets = { + planning_group_id_from_selector(group): PoseStamped( + frame_id="world", + position=pose.position, + orientation=pose.orientation, + ) + for group, pose in pose_targets.items() + } + auxiliary_ids = tuple(planning_group_id_from_selector(group) for group in auxiliary_groups) + group_ids = tuple(dict.fromkeys((*stamped_targets.keys(), *auxiliary_ids))) + planned_group_ids = self._begin_planning(group_ids) + if planned_group_ids is None: + return False + group_ids = planned_group_ids + + try: + start = self._selected_joint_state(group_ids) + except Exception as exc: + return self._fail(f"Failed to resolve planning groups: {exc}") + if start is None: return self._fail("No joint state") - ik = self._solve_ik_for_pose(robot_id, pose, current, check_collision=True) + ik = self._kinematics.solve_pose_targets( + world=self._world_monitor.world, + pose_targets={ + self._world_monitor.planning_groups.get(group_id): pose + for group_id, pose in stamped_targets.items() + }, + auxiliary_groups=tuple( + self._world_monitor.planning_groups.get(group_id) for group_id in auxiliary_ids + ), + seed=start, + check_collision=True, + ) if not ik.is_success() or ik.joint_state is None: return self._fail(f"IK failed: {ik.status.name}") - - logger.info(f"IK solved, error: {ik.position_error:.4f}m") - return self._plan_path_only(robot_name, robot_id, ik.joint_state, planning_epoch) + return self._plan_selected_path(group_ids, start, ik.joint_state) @rpc def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None) -> bool: - """Plan motion to joint config. Use preview_path() then execute(). + """Plan motion to joint config. Use preview_plan() then execute(). Args: joints: Target joint state (names + positions) robot_name: Robot to plan for (required if multiple robots configured) """ - if (r := self._begin_planning(robot_name)) is None: + robot = self._get_robot(robot_name) + if robot is None: return False - robot_name, robot_id = r - planning_epoch = self._planning_epoch + robot_name, _, _, _ = robot logger.info(f"Planning to joints for {robot_name}: {[f'{j:.3f}' for j in joints.position]}") - return self._plan_path_only(robot_name, robot_id, joints, planning_epoch) + group_id = self._default_group_id_for_robot(robot_name) + if group_id is None: + return False + return self.plan_to_joint_targets({group_id: joints}) - def _plan_path_only( - self, - robot_name: RobotName, - robot_id: WorldRobotID, - goal: JointState, - planning_epoch: int, + @rpc + def plan_to_joint_targets( + self, joint_targets: Mapping[PlanningGroupID | PlanningGroup, JointState] ) -> bool: - """Plan path from current position to goal, store result.""" - assert self._world_monitor and self._planner # guaranteed by _begin_planning - self._dismiss_preview(robot_id) - start = self._world_monitor.get_current_joint_state(robot_id) - if start is None: - return self._fail("No joint state") - - # Trim goal to planner DOF (e.g. strip gripper joint from coordinator state) - planner_dof = len(start.position) - if len(goal.position) > planner_dof: - goal = JointState( - name=list(goal.name[:planner_dof]) if goal.name else [], - position=list(goal.position[:planner_dof]), - ) + """Plan to joint targets keyed by planning group.""" + if self._world_monitor is None or self._planner is None: + return False + if not joint_targets: + return self._fail("At least one joint target is required") - result = self._planner.plan_joint_path( - world=self._world_monitor.world, - robot_id=robot_id, - start=start, - goal=goal, - timeout=self.config.planning_timeout, + group_ids = tuple( + dict.fromkeys(planning_group_id_from_selector(group) for group in joint_targets) ) - if self._state != ManipulationState.PLANNING or planning_epoch != self._planning_epoch: - logger.info("Discarding cancelled planning result") + planned_group_ids = self._begin_planning(group_ids) + if planned_group_ids is None: return False - if not result.is_success(): - return self._fail(f"Planning failed: {result.status.name}") - - logger.info(f"Path: {len(result.path)} waypoints") - self._planned_paths[robot_name] = result.path + group_ids = planned_group_ids + try: + start = self._selected_joint_state(group_ids) + except Exception as exc: + return self._fail(f"Failed to resolve planning groups: {exc}") + if start is None: + return self._fail("No joint state") - _, _, traj_gen = self._robots[robot_name] - # Convert JointState path to list of position lists for trajectory generator - traj = traj_gen.generate([list(state.position) for state in result.path]) - self._planned_trajectories[robot_name] = traj - logger.info(f"Trajectory: {traj.duration:.3f}s") + goal_names: list[str] = [] + goal_positions: list[float] = [] + for group, target in joint_targets.items(): + group_id = planning_group_id_from_selector(group) + target_global = self._joint_target_to_global_names(group_id, target) + if target_global is None: + return self._fail(f"Invalid joint target for '{group_id}'") + goal_names.extend(target_global.name) + goal_positions.extend(target_global.position) - self._state = ManipulationState.COMPLETED - return True + goal = JointState(name=goal_names, position=goal_positions) + return self._plan_selected_path(group_ids, start, goal) @rpc - def preview_path( + def preview_plan( self, + plan: GeneratedPlan | None = None, duration: float | None = None, robot_name: RobotName | None = None, - target_fps: float = 30.0, ) -> bool: - """Preview the planned path in the visualizer. - - Args: - duration: Total animation duration in seconds. Uses trajectory duration if None. - robot_name: Robot to preview (required if multiple robots configured) - target_fps: Nominal preview update rate. Set <= 0 to use planned waypoints directly. - """ + """Preview a generated plan, defaulting to `_last_plan` when omitted.""" if self._world_monitor is None: return False - - robot = self._get_robot(robot_name) - if robot is None: + plan = plan or self._last_plan + if plan is None or not plan.path: + logger.warning("No generated plan to preview") return False - robot_name, robot_id, _, _ = robot - - planned_path = self._planned_paths.get(robot_name) - if planned_path is None or len(planned_path) == 0: - logger.warning(f"No planned path to preview for {robot_name}") + if robot_name is not None and robot_name not in self._affected_robot_names(plan): + logger.error("Generated plan does not affect robot '%s'", robot_name) return False - - if duration is None: - trajectory = self._planned_trajectories.get(robot_name) - animation_duration = trajectory.duration if trajectory is not None else 3.0 - else: - trajectory = self._planned_trajectories.get(robot_name) - animation_duration = duration - - interpolated = list(planned_path) - if trajectory is not None and target_fps > 0 and animation_duration > 0: - times = np.array( - [point.time_from_start for point in trajectory.points], dtype=np.float64 - ) - positions = np.array([point.positions for point in trajectory.points], dtype=np.float64) - if len(times) > 1 and positions.ndim == 2 and times[-1] > times[0]: - frame_count = int(np.ceil(animation_duration * target_fps)) + 1 - sample_times = np.linspace(times[0], times[-1], frame_count) - joint_names = trajectory.joint_names or planned_path[0].name - sampled_positions = np.column_stack( - [ - np.interp(sample_times, times, positions[:, joint]) - for joint in range(positions.shape[1]) - ] - ) - interpolated = [ - JointState(name=joint_names, position=position.tolist()) - for position in sampled_positions - ] - self._world_monitor.animate_path(robot_id, interpolated, animation_duration) + animation_duration = duration if duration is not None else 1.0 + self._world_monitor.animate_plan(plan, animation_duration) return True @rpc @@ -708,13 +867,7 @@ def has_planned_path(self) -> bool: Returns: True if a path is planned and ready """ - robot = self._get_robot() - if robot is None: - return False - robot_name, _, _, _ = robot - - path = self._planned_paths.get(robot_name) - return path is not None and len(path) > 0 + return self._last_plan is not None and bool(self._last_plan.path) @rpc def get_visualization_url(self) -> str | None: @@ -734,15 +887,7 @@ def clear_planned_path(self) -> bool: Returns: True if cleared """ - if self._world_monitor is None: - return False - robot = self._get_robot() - if robot is None: - return False - robot_name, _, _, _ = robot - - self._planned_paths.pop(robot_name, None) - self._planned_trajectories.pop(robot_name, None) + self._last_plan = None return True @rpc @@ -769,16 +914,33 @@ def get_robot_info(self, robot_name: RobotName | None = None) -> dict[str, Any] return None robot_name, robot_id, config, _ = robot + planning_groups = ( + [ + { + "id": group.id, + "name": group.group_name, + "joint_names": list(group.joint_names), + "local_joint_names": list(group.local_joint_names), + "base_link": group.base_link, + "tip_link": group.tip_link, + "source": group.source, + "has_pose_target": group.has_pose_target, + } + for group in self._world_monitor.planning_groups.groups_for_robot(robot_name) + ] + if self._world_monitor is not None + else [] + ) return { "name": config.name, "world_robot_id": robot_id, "joint_names": config.joint_names, + "planning_groups": planning_groups, "end_effector_link": config.end_effector_link, "base_link": config.base_link, "max_velocity": config.max_velocity, "max_acceleration": config.max_acceleration, - "has_joint_name_mapping": bool(config.joint_name_mapping), "coordinator_task_name": config.coordinator_task_name, "home_joints": config.home_joints, "pre_grasp_offset": config.pre_grasp_offset, @@ -854,7 +1016,9 @@ def evaluate_joint_target( "joint_state": target, } - def evaluate_pose_target(self, pose: Pose, robot_name: RobotName) -> TargetEvaluation: + def evaluate_pose_target( + self, pose: Pose, robot_name: RobotName, check_collision: bool = True + ) -> TargetEvaluation: """Evaluate a Cartesian target for visualization without planning a path.""" robot_id = self.robot_id_for_name(robot_name) if robot_id is None: @@ -882,10 +1046,15 @@ def evaluate_pose_target(self, pose: Pose, robot_name: RobotName) -> TargetEvalu "message": "Planning is not initialized or current state is unavailable", "collision_free": False, } - ik = self._solve_ik_for_pose(robot_id, pose, current, check_collision=True) + ik = self._solve_ik_for_pose(robot_id, pose, current, check_collision=check_collision) joint_state = JointState(ik.joint_state) if ik.is_success() and ik.joint_state else None - collision_free = bool( - joint_state is not None and self._world_monitor.is_state_valid(robot_id, joint_state) + collision_free = ( + bool(joint_state is not None) + if not check_collision + else bool( + joint_state is not None + and self._world_monitor.is_state_valid(robot_id, joint_state) + ) ) return { "success": joint_state is not None and collision_free, @@ -897,19 +1066,318 @@ def evaluate_pose_target(self, pose: Pose, robot_name: RobotName) -> TargetEvalu "collision_free": collision_free, } - def get_planned_path(self, robot_name: RobotName) -> JointPath | None: - """Return a copy of the stored planned path for visualization.""" - path = self._planned_paths.get(robot_name) - if path is None: + def _selected_target_by_name( + self, group_ids: tuple[PlanningGroupID, ...], target_joints: JointState + ) -> dict[str, float] | None: + """Validate and index a global target joint state for a group selection.""" + assert self._world_monitor is not None + selection = self._world_monitor.planning_groups.select(group_ids) + if len(target_joints.name) != len(target_joints.position): + logger.error( + "Target set has %d names but %d positions", + len(target_joints.name), + len(target_joints.position), + ) + return None + try: + assert_global_joint_names(target_joints.name) + except ValueError as exc: + logger.error("Invalid target-set joint names: %s", exc) return None - return [JointState(point) for point in path] - def get_planned_trajectory_duration(self, robot_name: RobotName) -> float | None: - """Return the stored planned trajectory duration for visualization.""" - trajectory = self._planned_trajectories.get(robot_name) - if trajectory is None: + target_by_name = dict(zip(target_joints.name, target_joints.position, strict=True)) + missing = [name for name in selection.joint_names if name not in target_by_name] + extra = set(target_by_name) - set(selection.joint_names) + if missing: + logger.error("Target set missing joints: %s", missing) + return None + if extra: + logger.error("Target set has extra joints: %s", sorted(extra)) + return None + return target_by_name + + def _local_target_states_for_selection( + self, group_ids: tuple[PlanningGroupID, ...], target_joints: JointState + ) -> dict[RobotName, tuple[WorldRobotID, RobotModelConfig, JointState]] | None: + """Project selected global targets to full local robot states for evaluation.""" + assert self._world_monitor is not None + selection = self._world_monitor.planning_groups.select(group_ids) + target_by_name = self._selected_target_by_name(group_ids, target_joints) + if target_by_name is None: return None - return float(trajectory.duration) + + local_targets: dict[RobotName, tuple[WorldRobotID, RobotModelConfig, JointState]] = {} + for robot_name in selection.robot_names: + robot = self._robots.get(robot_name) + if robot is None: + logger.error("Robot '%s' is not registered", robot_name) + return None + robot_id, config, _ = robot + current = self._world_monitor.get_current_joint_state(robot_id) + if current is None: + logger.error("No joint state for robot '%s'", robot_name) + return None + current_by_name = self._current_positions_by_name(robot_name, current) + if current_by_name is None: + return None + + positions: list[float] = [] + for local_name, global_name in zip( + config.joint_names, + make_global_joint_names(robot_name, config.joint_names), + strict=True, + ): + if global_name in target_by_name: + positions.append(float(target_by_name[global_name])) + elif local_name in current_by_name: + positions.append(float(current_by_name[local_name])) + else: + logger.error("Current state missing joint '%s'", global_name) + return None + local_targets[robot_name] = ( + robot_id, + config, + JointState(name=list(config.joint_names), position=positions), + ) + return local_targets + + def _target_set_poses( + self, + group_ids: tuple[PlanningGroupID, ...], + local_targets: Mapping[RobotName, tuple[WorldRobotID, RobotModelConfig, JointState]], + ) -> dict[PlanningGroupID, PoseStamped | None]: + """Compute pose outputs for pose-targetable groups in a target set.""" + assert self._world_monitor is not None + poses: dict[PlanningGroupID, PoseStamped | None] = {} + for group in self._world_monitor.planning_groups.select(group_ids).groups: + if not group.has_pose_target: + continue + robot_target = local_targets.get(group.robot_name) + if robot_target is None: + poses[group.id] = None + continue + _, _, joint_state = robot_target + try: + poses[group.id] = self._world_monitor.get_group_pose(group.id, joint_state) + except Exception as exc: + logger.warning("Failed to evaluate pose for group '%s': %s", group.id, exc) + poses[group.id] = None + return poses + + def _evaluate_global_target_set( + self, + group_ids: tuple[PlanningGroupID, ...], + target_joints: JointState | None, + *, + status: str = "FEASIBLE", + message: str = "Target set is feasible", + position_error: float = 0.0, + orientation_error: float = 0.0, + check_collision: bool = True, + ) -> TargetSetEvaluation: + """Evaluate whole-set collision and pose outputs for global target joints.""" + if self._world_monitor is None: + return { + "success": False, + "status": "UNAVAILABLE", + "message": "Planning is not initialized", + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + if target_joints is None: + return { + "success": False, + "status": "NO_TARGET", + "message": "No target joints provided", + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + try: + local_targets = self._local_target_states_for_selection(group_ids, target_joints) + except (KeyError, ValueError) as exc: + return { + "success": False, + "status": "INVALID", + "message": str(exc), + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + if local_targets is None: + return { + "success": False, + "status": "INVALID", + "message": "Invalid target-set joints", + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + + collision_free = True + diagnostics: dict[PlanningGroupID, str] = {} + selection = self._world_monitor.planning_groups.select(group_ids) + for robot_name, (robot_id, _, local_target) in local_targets.items(): + robot_collision_free = ( + self._world_monitor.is_state_valid(robot_id, local_target) + if check_collision + else True + ) + collision_free = collision_free and robot_collision_free + for group in selection.groups: + if group.robot_name == robot_name: + if not check_collision: + diagnostics[group.id] = "Target collision check skipped" + else: + diagnostics[group.id] = ( + "Target is collision-free" + if robot_collision_free + else "Target is in collision" + ) + + return { + "success": collision_free, + "status": status if collision_free else "COLLISION", + "message": message if collision_free else "Target set is in collision", + "collision_free": collision_free, + "group_ids": group_ids, + "target_joints": JointState(target_joints), + "group_diagnostics": diagnostics, + "group_poses": self._target_set_poses(group_ids, local_targets), + "position_error": position_error, + "orientation_error": orientation_error, + } + + def evaluate_joint_target_set( + self, joint_targets: Mapping[PlanningGroupID | PlanningGroup, JointState] + ) -> TargetSetEvaluation: + """Evaluate joint targets for a whole planning target set.""" + if self._world_monitor is None: + return { + "success": False, + "status": "UNAVAILABLE", + "message": "Planning is not initialized", + "collision_free": False, + "target_joints": None, + } + if not joint_targets: + return { + "success": False, + "status": "NO_TARGET", + "message": "No joint targets provided", + "collision_free": False, + "target_joints": None, + } + + group_ids = tuple( + dict.fromkeys(planning_group_id_from_selector(group) for group in joint_targets) + ) + goal_names: list[str] = [] + goal_positions: list[float] = [] + for group_selector, target in joint_targets.items(): + group_id = planning_group_id_from_selector(group_selector) + target_global = self._joint_target_to_global_names(group_id, target) + if target_global is None: + return { + "success": False, + "status": "INVALID", + "message": f"Invalid joint target for '{group_id}'", + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + goal_names.extend(target_global.name) + goal_positions.extend(target_global.position) + + return self._evaluate_global_target_set( + group_ids, + JointState(name=goal_names, position=goal_positions), + ) + + def evaluate_pose_target_set( + self, + pose_targets: Mapping[PlanningGroupID | PlanningGroup, Pose | PoseStamped], + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroup] = (), + seed: JointState | None = None, + check_collision: bool = True, + ) -> TargetSetEvaluation: + """Evaluate pose targets for a whole planning target set using configured IK.""" + if self._world_monitor is None or self._kinematics is None: + return { + "success": False, + "status": "UNAVAILABLE", + "message": "Planning is not initialized", + "collision_free": False, + "target_joints": None, + } + if not pose_targets: + return { + "success": False, + "status": "NO_TARGET", + "message": "No pose targets provided", + "collision_free": False, + "target_joints": None, + } + + def stamped_pose(pose: Pose | PoseStamped) -> PoseStamped: + if isinstance(pose, PoseStamped): + return pose + return PoseStamped( + frame_id="world", position=pose.position, orientation=pose.orientation + ) + + stamped_targets = { + planning_group_id_from_selector(group): stamped_pose(pose) + for group, pose in pose_targets.items() + } + auxiliary_ids = tuple(planning_group_id_from_selector(group) for group in auxiliary_groups) + group_ids = tuple(dict.fromkeys((*stamped_targets.keys(), *auxiliary_ids))) + try: + target_groups = { + self._world_monitor.planning_groups.get(group_id): pose + for group_id, pose in stamped_targets.items() + } + auxiliary = tuple( + self._world_monitor.planning_groups.get(group_id) for group_id in auxiliary_ids + ) + ik = self._kinematics.solve_pose_targets( + world=self._world_monitor.world, + pose_targets=target_groups, + auxiliary_groups=auxiliary, + seed=seed or self._selected_joint_state(group_ids), + check_collision=False, + ) + except (KeyError, ValueError) as exc: + return { + "success": False, + "status": "INVALID", + "message": str(exc), + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + } + + if not ik.is_success() or ik.joint_state is None: + return { + "success": False, + "status": ik.status.name, + "message": ik.message, + "collision_free": False, + "group_ids": group_ids, + "target_joints": None, + "position_error": ik.position_error, + "orientation_error": ik.orientation_error, + } + return self._evaluate_global_target_set( + group_ids, + ik.joint_state, + status=ik.status.name, + message=ik.message, + position_error=ik.position_error, + orientation_error=ik.orientation_error, + check_collision=check_collision, + ) @rpc def set_init_joints(self, joint_state: JointState, robot_name: RobotName | None = None) -> bool: @@ -922,13 +1390,46 @@ def set_init_joints(self, joint_state: JointState, robot_name: RobotName | None robot = self._get_robot(robot_name) if robot is None: return False - self._init_joints[robot[0]] = joint_state + robot_name_resolved, _, config, _ = robot + try: + normalized = self._local_robot_joint_state(config, joint_state) + except ValueError as exc: + logger.error(str(exc)) + return False + self._init_joints[robot_name_resolved] = normalized logger.info( - f"Init joints set for '{robot[0]}': " - f"[{', '.join(f'{j:.3f}' for j in joint_state.position)}]" + f"Init joints set for '{robot_name_resolved}': " + f"[{', '.join(f'{j:.3f}' for j in normalized.position)}]" ) return True + def _local_robot_joint_state( + self, config: RobotModelConfig, joint_state: JointState + ) -> JointState: + """Normalize a robot-scoped joint state to local model joint order.""" + if not joint_state.name: + if len(joint_state.position) != len(config.joint_names): + raise ValueError( + f"JointState has {len(joint_state.position)} positions, " + f"expected {len(config.joint_names)} for robot '{config.name}'" + ) + return JointState(name=list(config.joint_names), position=list(joint_state.position)) + + assert_local_joint_names(joint_state.name) + positions_by_name = dict(zip(joint_state.name, joint_state.position, strict=False)) + missing = [name for name in config.joint_names if name not in positions_by_name] + if missing: + raise ValueError(f"JointState for robot '{config.name}' is missing joints: {missing}") + extra = set(joint_state.name) - set(config.joint_names) + if extra: + raise ValueError( + f"JointState for robot '{config.name}' has extra joints: {sorted(extra)}" + ) + return JointState( + name=list(config.joint_names), + position=[positions_by_name[name] for name in config.joint_names], + ) + @rpc def set_init_joints_to_current(self, robot_name: RobotName | None = None) -> bool: """Set init joints to the current joint positions. @@ -966,72 +1467,167 @@ def _get_coordinator_client(self) -> RPCClient | None: self._coordinator_client = RPCClient(None, ControlCoordinator) return self._coordinator_client - def _translate_trajectory_to_coordinator( + def _invoke_coordinator_task( self, - trajectory: JointTrajectory, - robot_config: RobotModelConfig, - ) -> JointTrajectory: - """Translate trajectory joint names from URDF to coordinator namespace. - - Args: - trajectory: Trajectory with URDF joint names - robot_config: Robot config with joint name mapping - - Returns: - Trajectory with coordinator joint names - """ - if not robot_config.joint_name_mapping: - return trajectory # No translation needed - - # Translate joint names - coordinator_names = [ - robot_config.get_coordinator_joint_name(j) for j in trajectory.joint_names - ] - - # Create new trajectory with translated names - # Note: duration is computed automatically from points in JointTrajectory.__init__ - return JointTrajectory( - joint_names=coordinator_names, - points=trajectory.points, - timestamp=trajectory.timestamp, - ) + client: RPCClient, + task_name: str, + method: str, + kwargs: dict[str, Any], + ) -> Any: + """Invoke a ControlCoordinator task with an execution-specific timeout.""" + remote_name = getattr(client, "remote_name", None) + rpc_client = getattr(client, "rpc", None) + call_sync = getattr(rpc_client, "call_sync", None) + if isinstance(remote_name, str) and callable(call_sync): + result, unsub_fn = call_sync( + f"{remote_name}/task_invoke", + ([task_name, method, kwargs], {}), + rpc_timeout=self.config.coordinator_rpc_timeout, + ) + unsub_fns = getattr(client, "_unsub_fns", None) + if isinstance(unsub_fns, list): + unsub_fns.append(unsub_fn) + return result + return client.task_invoke(task_name, method, kwargs) @rpc - def execute(self, robot_name: RobotName | None = None) -> bool: + def execute(self) -> bool: """Execute planned trajectory via ControlCoordinator.""" - if (robot := self._get_robot(robot_name)) is None: - return False - robot_name, _, config, _ = robot + return self.execute_plan(self._last_plan) - if (traj := self._planned_trajectories.get(robot_name)) is None: - logger.warning("No planned trajectory") - return False - if not config.coordinator_task_name: - logger.error(f"No coordinator_task_name for '{robot_name}'") + @rpc + def execute_plan(self, plan: GeneratedPlan | None = None) -> bool: + """Project and execute a generated plan through affected trajectory tasks. + + TODO: proper time parametrization. + """ + plan = plan or self._last_plan + if plan is None or not plan.path: + logger.warning("No generated plan") return False if (client := self._get_coordinator_client()) is None: logger.error("No coordinator client") return False - translated = self._translate_trajectory_to_coordinator(traj, config) + try: + affected = self._affected_robot_names(plan) + except Exception as exc: + return self._fail(f"Failed to resolve generated plan: {exc}") logger.info( - f"Executing: task='{config.coordinator_task_name}', {len(translated.points)} pts, {translated.duration:.2f}s" + "Execute plan: groups=%s, affected=%s", + plan.group_ids, + affected, ) + assert self._world_monitor is not None + + dispatches: list[tuple[RobotName, RobotModelConfig, JointTrajectory]] = [] + for name in affected: + robot = self._get_robot(name) + if robot is None: + return False + resolved_name, robot_id, config, traj_gen = robot + if not config.coordinator_task_name: + logger.error(f"No coordinator_task_name for '{resolved_name}'") + return False + + current_by_name: dict[str, float] = {} + current = self._world_monitor.get_current_joint_state(robot_id) + if current is not None: + indexed_current = self._current_positions_by_name(resolved_name, current) + if indexed_current is None: + return False + current_by_name = indexed_current + + global_joint_names = make_global_joint_names(resolved_name, config.joint_names) + local_path: list[JointState] = [] + for waypoint in plan.path: + if len(waypoint.name) != len(waypoint.position): + logger.error( + "Cannot execute plan for '%s': waypoint has %d names but %d positions", + resolved_name, + len(waypoint.name), + len(waypoint.position), + ) + return False + try: + assert_global_joint_names(waypoint.name) + except ValueError as exc: + logger.error("Cannot execute plan for '%s': %s", resolved_name, exc) + return False + selected_positions = dict(zip(waypoint.name, waypoint.position, strict=True)) + positions: list[float] = [] + for local_name, global_name in zip( + config.joint_names, global_joint_names, strict=True + ): + if global_name in selected_positions: + positions.append(selected_positions[global_name]) + elif local_name in current_by_name: + positions.append(current_by_name[local_name]) + else: + logger.error( + "Cannot execute plan for '%s': missing joint '%s'", + resolved_name, + global_name, + ) + return False + local_path.append(JointState(name=list(config.joint_names), position=positions)) + if len(local_path) < 2: + logger.error("Plan projection for '%s' has fewer than two waypoints", resolved_name) + return False + local_trajectory = traj_gen.generate([list(state.position) for state in local_path]) + trajectory = JointTrajectory( + joint_names=list(global_joint_names), + points=local_trajectory.points, + timestamp=local_trajectory.timestamp, + ) + dispatches.append((resolved_name, config, trajectory)) self._state = ManipulationState.EXECUTING - result = client.task_invoke( - config.coordinator_task_name, "execute", {"trajectory": translated} - ) - if result: - logger.info("Trajectory accepted") - self._state = ManipulationState.COMPLETED - return True - else: - return self._fail("Coordinator rejected trajectory") + for _name, config, trajectory in dispatches: + logger.info( + "Executing: task='%s', %d pts, %.2fs", + config.coordinator_task_name, + len(trajectory.points), + trajectory.duration, + ) + try: + result = self._invoke_coordinator_task( + client, + config.coordinator_task_name, + "execute", + {"trajectory": trajectory}, + ) + except TimeoutError as exc: + return self._fail( + f"Coordinator RPC timed out for task '{config.coordinator_task_name}': {exc}" + ) + except Exception as exc: + return self._fail( + f"Coordinator RPC failed for task '{config.coordinator_task_name}': {exc}" + ) + logger.info( + "Coordinator execute result: task='%s', result=%r", + config.coordinator_task_name, + result, + ) + if not result: + return self._fail("Coordinator rejected trajectory") + + logger.info("Trajectory accepted") + self._state = ManipulationState.COMPLETED + return True @rpc def get_trajectory_status(self, robot_name: RobotName | None = None) -> dict[str, Any] | None: """Get trajectory execution status via coordinator task_invoke.""" + last_plan = self._last_plan + if robot_name is None and last_plan is not None and last_plan.path: + statuses = { + name: self.get_trajectory_status(name) + for name in self._affected_robot_names(last_plan) + } + return {"robots": statuses} + if (robot := self._get_robot(robot_name)) is None: return None _, _, config, _ = robot @@ -1080,9 +1676,6 @@ def add_obstacle( logger.warning("mesh_path required for mesh obstacles") return "" - # Import PoseStamped here to avoid circular imports - from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped - obstacle = Obstacle( name=name, obstacle_type=obstacle_type, @@ -1189,6 +1782,18 @@ def _wait_for_trajectory_completion( Returns: True if trajectory completed successfully """ + last_plan = self._last_plan + if robot_name is None and last_plan is not None and last_plan.path: + try: + robot_names = self._affected_robot_names(last_plan) + except Exception as exc: + logger.warning("Failed to resolve generated plan while waiting: %s", exc) + return False + return all( + self._wait_for_trajectory_completion(name, timeout, poll_interval) + for name in robot_names + ) + robot = self._get_robot(robot_name) if robot is None: return True @@ -1196,11 +1801,7 @@ def _wait_for_trajectory_completion( client = self._get_coordinator_client() if client is None or not config.coordinator_task_name: - # No coordinator — wait for trajectory duration as fallback - traj = self._planned_trajectories.get(rname) - if traj is not None: - logger.info(f"No coordinator status — waiting {traj.duration:.1f}s for trajectory") - time.sleep(traj.duration + 0.5) + logger.info("No coordinator status available for '%s'", rname) return True # Poll task state via task_invoke @@ -1221,13 +1822,7 @@ def _wait_for_trajectory_completion( # task_invoke returned None — task not found, assume done return True except Exception: - # Fallback: wait for trajectory duration - traj = self._planned_trajectories.get(rname) - if traj is not None: - remaining = traj.duration - (time.time() - start) - if remaining > 0: - logger.info(f"Status poll failed — waiting {remaining:.1f}s for trajectory") - time.sleep(remaining + 0.5) + logger.info("Status poll failed for '%s'", rname) return True time.sleep(poll_interval) @@ -1262,10 +1857,10 @@ def _preview_execute_wait( preview_duration: Duration to animate the preview in Meshcat (seconds) """ logger.info("Previewing trajectory...") - self.preview_path(preview_duration, robot_name) + self.preview_plan(duration=preview_duration, robot_name=robot_name) logger.info("Executing trajectory...") - if not self.execute(robot_name): + if not self.execute(): return SkillResult.fail("EXECUTION_FAILED", "Trajectory execution failed") if not self._wait_for_trajectory_completion(robot_name): diff --git a/dimos/manipulation/planning/README.md b/dimos/manipulation/planning/README.md index 6e5a53eb57..96d8176268 100644 --- a/dimos/manipulation/planning/README.md +++ b/dimos/manipulation/planning/README.md @@ -70,7 +70,6 @@ config = RobotModelConfig( joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"], end_effector_link="link7", base_link="link_base", - joint_name_mapping={"arm_joint1": "joint1", ...}, # coordinator <-> URDF coordinator_task_name="traj_arm", ) @@ -93,12 +92,11 @@ module.execute() # Sends to coordinator | `name` | Robot identifier | | `model_path` | Path to URDF/XACRO file | | `base_pose` | PoseStamped for robot base in world frame | -| `joint_names` | Joint names in URDF | +| `joint_names` | Ordered controllable local model joint names | | `end_effector_link` | EE link name | | `base_link` | Base link name | | `max_velocity` | Max joint velocity (rad/s) | | `max_acceleration` | Max acceleration (rad/s²) | -| `joint_name_mapping` | Coordinator → URDF name mapping | | `coordinator_task_name` | Task name for execution RPC | | `package_paths` | ROS package paths for meshes | | `xacro_args` | Xacro arguments (e.g., `{"dof": "7"}`) | @@ -141,7 +139,7 @@ accepted. |-----------|-------------| | `xarm6_planner_only` | XArm 6-DOF standalone (no coordinator) | | `xarm7-planner-coordinator` | XArm 7-DOF with coordinator | -| `dual-xarm6-planner` | Dual XArm 6-DOF | +| `dual-xarm6-planner-coordinator` | Dual XArm 6-DOF with coordinator | | `xarm-perception-sim` | XArm 7-DOF simulation perception stack | ## Directory Structure diff --git a/dimos/manipulation/planning/examples/manipulation_client.py b/dimos/manipulation/planning/examples/manipulation_client.py index 1185f28f21..57ba11b002 100644 --- a/dimos/manipulation/planning/examples/manipulation_client.py +++ b/dimos/manipulation/planning/examples/manipulation_client.py @@ -163,15 +163,14 @@ def plan_pose( def preview( duration: float | None = None, robot_name: str | None = None, - target_fps: float = 30.0, ) -> bool: - """Preview planned path in Meshcat.""" - return _client.preview_path(duration, robot_name, target_fps) + """Preview the last generated plan in Visualizer.""" + return _client.preview_plan(None, duration, robot_name) -def execute(robot_name: str | None = None) -> bool: +def execute() -> bool: """Execute planned trajectory via coordinator.""" - return _client.execute(robot_name) + return _client.execute() def home(robot_name: str | None = None) -> bool: @@ -181,7 +180,7 @@ def home(robot_name: str | None = None) -> bool: home_joints = _client.get_robot_info(robot_name).get("home_joints", [0.0] * 7) success = _client.plan_to_joints(JointState(position=home_joints), robot_name) if success: - return _client.execute(robot_name) + return _client.execute() return False diff --git a/dimos/manipulation/planning/groups/discovery.py b/dimos/manipulation/planning/groups/discovery.py new file mode 100644 index 0000000000..5e9ddfa25d --- /dev/null +++ b/dimos/manipulation/planning/groups/discovery.py @@ -0,0 +1,358 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Planning group discovery from SRDF or conservative model fallback.""" + +from __future__ import annotations + +import itertools +from pathlib import Path +import warnings +import xml.etree.ElementTree as ET + +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition +from dimos.robot.model_parser import JointDescription, ModelDescription +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +FALLBACK_PLANNING_GROUP_NAME = "manipulator" + + +class PlanningGroupDiscoveryError(ValueError): + """Raised when planning groups cannot be discovered for a model.""" + + +def _warn(message: str) -> None: + logger.warning(message) + warnings.warn(message, UserWarning, stacklevel=2) + + +def discover_planning_group_definitions( + *, + robot_name: str, + model_path: Path, + model: ModelDescription, + controllable_joint_names: list[str], + srdf_path: Path | None = None, +) -> list[PlanningGroupDefinition]: + """Discover planning groups from SRDF or fallback generation. + + Precedence is explicit SRDF path, conservative auto-discovery with warning, + then fallback generation from the controllable joint set. + """ + resolved_srdf_path = _resolve_srdf_path(model_path, srdf_path) + if resolved_srdf_path is not None: + groups = parse_srdf_planning_groups( + resolved_srdf_path, + model=model, + controllable_joint_names=controllable_joint_names, + ) + if groups: + return groups + _warn( + f"No supported planning groups found in SRDF {resolved_srdf_path} " + f"for robot {robot_name}; trying fallback generation" + ) + + return [ + generate_fallback_planning_group( + model=model, + controllable_joint_names=controllable_joint_names, + ) + ] + + +def parse_srdf_planning_groups( + srdf_path: Path, + *, + model: ModelDescription, + controllable_joint_names: list[str], +) -> list[PlanningGroupDefinition]: + """Extract supported SRDF planning group definitions. + + Supported forms are a single ```` + child or an ordered list of ```` children. Other forms, + including SRDF ```` metadata, are ignored for planning group + extraction. This is intentionally a minimal SRDF group extractor rather + than a full SRDF parser; adopting a ROS/MoveIt parser such as srdfdom would + add substantial dependency overhead for this narrow subset. + """ + root = ET.parse(srdf_path).getroot() + groups: list[PlanningGroupDefinition] = [] + for group_elem in root.findall("group"): + group_name = group_elem.get("name") + if not group_name: + _warn(f"Skipping SRDF group without a name in {srdf_path}") + continue + + children = [child for child in list(group_elem) if isinstance(child.tag, str)] + chain_children = [child for child in children if child.tag == "chain"] + joint_children = [child for child in children if child.tag == "joint"] + unsupported_children = [child for child in children if child.tag not in {"chain", "joint"}] + + if len(chain_children) == 1 and not joint_children and not unsupported_children: + definition = _parse_chain_group( + group_name, + chain_children[0], + model=model, + controllable_joint_names=controllable_joint_names, + srdf_path=srdf_path, + ) + elif joint_children and len(joint_children) == len(children): + definition = _parse_joint_list_group( + group_name, + joint_children, + model=model, + controllable_joint_names=controllable_joint_names, + srdf_path=srdf_path, + ) + else: + child_tags = [child.tag for child in children] + _warn( + f"Skipping unsupported SRDF planning group {group_name} in " + f"{srdf_path} with child tags {child_tags}" + ) + definition = None + + if definition is not None: + groups.append(definition) + + return groups + + +def generate_fallback_planning_group( + *, + model: ModelDescription, + controllable_joint_names: list[str], +) -> PlanningGroupDefinition: + """Generate one conservative fallback planning group named ``manipulator``.""" + ordered_joints = _validate_and_order_serial_joints(model, controllable_joint_names) + while ordered_joints and ordered_joints[-1].type == "prismatic": + removed = ordered_joints.pop() + _warn( + f"Excluding terminal prismatic joint {removed.name} from " + f"fallback planning group {FALLBACK_PLANNING_GROUP_NAME}" + ) + + if not ordered_joints: + raise PlanningGroupDiscoveryError( + "Fallback planning group generation removed all candidate joints; provide SRDF" + ) + + return PlanningGroupDefinition( + name=FALLBACK_PLANNING_GROUP_NAME, + joint_names=tuple(joint.name for joint in ordered_joints), + base_link=ordered_joints[0].parent_link, + tip_link=ordered_joints[-1].child_link, + source="fallback", + ) + + +def _resolve_srdf_path(model_path: Path, srdf_path: Path | None) -> Path | None: + if srdf_path is not None: + if srdf_path.exists(): + return srdf_path + raise FileNotFoundError(f"SRDF file not found: {srdf_path}") + + for candidate in _srdf_auto_discovery_candidates(model_path): + if candidate.exists(): + _warn(f"Auto-discovered SRDF at {candidate}") + return candidate + return None + + +def _srdf_auto_discovery_candidates(model_path: Path) -> list[Path]: + candidates: list[Path] = [] + name = model_path.name + if name.endswith(".urdf.xacro"): + candidates.append(model_path.with_name(name.removesuffix(".urdf.xacro") + ".srdf")) + elif model_path.suffix: + candidates.append(model_path.with_suffix(".srdf")) + candidates.append(model_path.parent / "config" / "robot.srdf") + candidates.append(model_path.parent.parent / "config" / "robot.srdf") + return list(dict.fromkeys(candidates)) + + +def _parse_chain_group( + group_name: str, + chain_elem: ET.Element, + *, + model: ModelDescription, + controllable_joint_names: list[str], + srdf_path: Path, +) -> PlanningGroupDefinition | None: + base_link = chain_elem.get("base_link") + tip_link = chain_elem.get("tip_link") + if not base_link or not tip_link: + _warn( + f"Skipping SRDF chain group {group_name} in {srdf_path} because " + "base_link or tip_link is missing" + ) + return None + + try: + ordered_joints = _ordered_joints_between_links(model, base_link, tip_link) + controlled_joints = [joint for joint in ordered_joints if joint.type != "fixed"] + _validate_controllable(group_name, controlled_joints, controllable_joint_names) + except PlanningGroupDiscoveryError as exc: + _warn(f"Skipping SRDF chain group {group_name} in {srdf_path}: {exc}") + return None + + return PlanningGroupDefinition( + name=group_name, + joint_names=tuple(joint.name for joint in controlled_joints), + base_link=base_link, + tip_link=tip_link, + source="srdf", + ) + + +def _parse_joint_list_group( + group_name: str, + joint_children: list[ET.Element], + *, + model: ModelDescription, + controllable_joint_names: list[str], + srdf_path: Path, +) -> PlanningGroupDefinition | None: + joint_names = [child.get("name", "") for child in joint_children] + if any(not name for name in joint_names): + _warn(f"Skipping SRDF joint-list group {group_name} in {srdf_path} with empty joint name") + return None + try: + ordered_joints = _validate_ordered_serial_joints(model, joint_names) + _validate_controllable(group_name, ordered_joints, controllable_joint_names) + except PlanningGroupDiscoveryError as exc: + _warn(f"Skipping SRDF joint-list group {group_name} in {srdf_path}: {exc}") + return None + + return PlanningGroupDefinition( + name=group_name, + joint_names=tuple(joint.name for joint in ordered_joints), + base_link=ordered_joints[0].parent_link, + tip_link=ordered_joints[-1].child_link, + source="srdf", + ) + + +def _ordered_joints_between_links( + model: ModelDescription, + base_link: str, + tip_link: str, +) -> list[JointDescription]: + joints_by_parent: dict[str, list[JointDescription]] = {} + for joint in model.joints: + joints_by_parent.setdefault(joint.parent_link, []).append(joint) + + ordered_joints: list[JointDescription] = [] + current_link = base_link + visited_links = {base_link} + while current_link != tip_link: + children = joints_by_parent.get(current_link, []) + if len(children) != 1: + raise PlanningGroupDiscoveryError( + f"chain from {base_link} to {tip_link} is branching or disconnected at {current_link}" + ) + joint = children[0] + ordered_joints.append(joint) + current_link = joint.child_link + if current_link in visited_links: + raise PlanningGroupDiscoveryError("chain contains a cycle") + visited_links.add(current_link) + + return ordered_joints + + +def _validate_ordered_serial_joints( + model: ModelDescription, + joint_names: list[str], +) -> list[JointDescription]: + ordered_joints: list[JointDescription] = [] + for joint_name in joint_names: + joint = model.get_joint(joint_name) + if joint is None: + raise PlanningGroupDiscoveryError(f"joint {joint_name} does not exist in model") + if joint.type == "fixed": + raise PlanningGroupDiscoveryError(f"joint {joint_name} is fixed") + ordered_joints.append(joint) + + if not ordered_joints: + raise PlanningGroupDiscoveryError("planning group contains no joints") + + for previous, current in itertools.pairwise(ordered_joints): + if previous.child_link != current.parent_link: + raise PlanningGroupDiscoveryError( + f"joints {previous.name} and {current.name} are not adjacent in a serial chain" + ) + return ordered_joints + + +def _validate_and_order_serial_joints( + model: ModelDescription, + joint_names: list[str], +) -> list[JointDescription]: + if not joint_names: + raise PlanningGroupDiscoveryError("fallback requires at least one controllable joint") + + joints: list[JointDescription] = [] + for joint_name in joint_names: + joint = model.get_joint(joint_name) + if joint is None: + raise PlanningGroupDiscoveryError(f"joint {joint_name} does not exist in model") + if joint.type == "fixed": + raise PlanningGroupDiscoveryError(f"joint {joint_name} is fixed") + joints.append(joint) + + by_parent = {joint.parent_link: joint for joint in joints} + by_child = {joint.child_link: joint for joint in joints} + if len(by_parent) != len(joints) or len(by_child) != len(joints): + raise PlanningGroupDiscoveryError("controllable joints branch or merge; provide SRDF") + + starts = [joint for joint in joints if joint.parent_link not in by_child] + ends = [joint for joint in joints if joint.child_link not in by_parent] + if len(starts) != 1 or len(ends) != 1: + raise PlanningGroupDiscoveryError( + "controllable joints are disconnected or cyclic; provide SRDF" + ) + + ordered_joints: list[JointDescription] = [] + current = starts[0] + while True: + ordered_joints.append(current) + next_joint = by_parent.get(current.child_link) + if next_joint is None: + break + current = next_joint + + if len(ordered_joints) != len(joints): + raise PlanningGroupDiscoveryError("controllable joints are disconnected; provide SRDF") + return ordered_joints + + +def _validate_controllable( + group_name: str, + joints: list[JointDescription], + controllable_joint_names: list[str], +) -> None: + if not joints: + raise PlanningGroupDiscoveryError( + f"planning group {group_name} contains no controllable joints" + ) + controllable = set(controllable_joint_names) + missing = [joint.name for joint in joints if joint.name not in controllable] + if missing: + raise PlanningGroupDiscoveryError( + f"planning group {group_name} includes joints outside controllable set: {missing}" + ) diff --git a/dimos/manipulation/planning/groups/models.py b/dimos/manipulation/planning/groups/models.py new file mode 100644 index 0000000000..c08b1bd4b5 --- /dev/null +++ b/dimos/manipulation/planning/groups/models.py @@ -0,0 +1,112 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Backend-independent planning-group domain models.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Literal, TypeAlias + +from dimos.manipulation.planning.spec.models import ( + GlobalJointName, + LocalModelJointName, + PlanningGroupID, + RobotName, +) + +PlanningGroupSource: TypeAlias = Literal["srdf", "fallback"] + + +@dataclass(frozen=True) +class PlanningGroupDefinition: + """Model-level declaration of a planning group. + + Joint names are local model names. The definition is safe to store on + ``RobotModelConfig`` and is not bound to any runtime world robot ID. + """ + + name: str + joint_names: tuple[LocalModelJointName, ...] + base_link: str + tip_link: str | None = None + source: PlanningGroupSource = "srdf" + + @property + def has_pose_target(self) -> bool: + """Whether this group has a valid pose target frame.""" + return self.tip_link is not None + + +@dataclass(frozen=True) +class PlanningGroup: + """Public backend-independent planning group. + + A planning group exposes stable public IDs and global joint names for + planning APIs. It intentionally does not include backend runtime robot IDs. + """ + + id: PlanningGroupID + robot_name: RobotName + group_name: str + joint_names: tuple[GlobalJointName, ...] + local_joint_names: tuple[LocalModelJointName, ...] + base_link: str + tip_link: str | None = None + source: PlanningGroupSource = "srdf" + + @property + def has_pose_target(self) -> bool: + """Whether this group can be directly pose-targeted.""" + return self.tip_link is not None + + +@dataclass(frozen=True) +class PlanningGroupSelection: + """Validated ordered selection of planning groups. + + Selection validates ID existence and selected-joint overlap outside any + world backend. Requested group order is preserved. + """ + + groups: tuple[PlanningGroup, ...] + group_ids: tuple[PlanningGroupID, ...] + joint_names: tuple[GlobalJointName, ...] + robot_names: tuple[RobotName, ...] + + @classmethod + def from_groups(cls, groups: tuple[PlanningGroup, ...]) -> PlanningGroupSelection: + """Build a selection, rejecting overlapping selected global joints.""" + seen_joints: dict[GlobalJointName, PlanningGroupID] = {} + joint_names: list[GlobalJointName] = [] + robot_names: list[RobotName] = [] + for group in groups: + if group.robot_name not in robot_names: + robot_names.append(group.robot_name) + for joint_name in group.joint_names: + previous_group_id = seen_joints.get(joint_name) + if previous_group_id is not None: + raise ValueError( + "Selected planning groups overlap on global joint " + f"{joint_name}: {previous_group_id} and {group.id}" + ) + seen_joints[joint_name] = group.id + joint_names.append(joint_name) + + return cls( + groups=groups, + group_ids=tuple(group.id for group in groups), + joint_names=tuple(joint_names), + robot_names=tuple(robot_names), + ) diff --git a/dimos/manipulation/planning/groups/registry.py b/dimos/manipulation/planning/groups/registry.py new file mode 100644 index 0000000000..4006f38a13 --- /dev/null +++ b/dimos/manipulation/planning/groups/registry.py @@ -0,0 +1,103 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Backend-independent planning-group registry.""" + +from __future__ import annotations + +from collections.abc import Iterable +from typing import TYPE_CHECKING + +from dimos.manipulation.planning.groups.discovery import FALLBACK_PLANNING_GROUP_NAME +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection +from dimos.manipulation.planning.planning_identifiers import ( + make_global_joint_names, + make_planning_group_id, +) +from dimos.manipulation.planning.spec.models import PlanningGroupID, RobotName + +if TYPE_CHECKING: + from dimos.manipulation.planning.spec.config import RobotModelConfig + + +class PlanningGroupRegistry: + """Registry of public planning groups derived from robot configs.""" + + def __init__(self, robot_configs: Iterable[RobotModelConfig] = ()) -> None: + self._groups: dict[PlanningGroupID, PlanningGroup] = {} + self._groups_by_robot: dict[RobotName, list[PlanningGroup]] = {} + for config in robot_configs: + self.add_robot(config) + + def add_robot(self, config: RobotModelConfig) -> None: + """Register all planning groups declared by one robot config.""" + if config.name in self._groups_by_robot: + raise ValueError(f"Robot '{config.name}' is already registered") + + robot_groups: list[PlanningGroup] = [] + for definition in config.planning_groups: + group_id = make_planning_group_id(config.name, definition.name) + if group_id in self._groups: + raise ValueError(f"Planning group '{group_id}' is already registered") + group = PlanningGroup( + id=group_id, + robot_name=config.name, + group_name=definition.name, + joint_names=tuple(make_global_joint_names(config.name, definition.joint_names)), + local_joint_names=definition.joint_names, + base_link=definition.base_link, + tip_link=definition.tip_link, + source=definition.source, + ) + self._groups[group_id] = group + robot_groups.append(group) + self._groups_by_robot[config.name] = robot_groups + + def list(self) -> tuple[PlanningGroup, ...]: + """List planning groups in robot registration order.""" + groups: list[PlanningGroup] = [] + for robot_groups in self._groups_by_robot.values(): + groups.extend(robot_groups) + return tuple(groups) + + def get(self, group_id: PlanningGroupID) -> PlanningGroup: + """Return one planning group by public ID.""" + try: + return self._groups[group_id] + except KeyError as exc: + raise KeyError(f"Unknown planning group ID: {group_id}") from exc + + def select(self, group_ids: Iterable[PlanningGroupID]) -> PlanningGroupSelection: + """Validate and return an ordered planning-group selection.""" + return PlanningGroupSelection.from_groups( + tuple(self.get(group_id) for group_id in group_ids) + ) + + def groups_for_robot(self, robot_name: RobotName) -> tuple[PlanningGroup, ...]: + """Return planning groups for one robot.""" + return tuple(self._groups_by_robot.get(robot_name, ())) + + def default_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: + """Return the generated fallback group ID for robot-scoped wrappers.""" + group_id = make_planning_group_id(robot_name, FALLBACK_PLANNING_GROUP_NAME) + return group_id if group_id in self._groups else None + + def primary_pose_group_id_for_robot(self, robot_name: RobotName) -> PlanningGroupID | None: + """Return the first pose-targetable group ID for compatibility paths.""" + # TODO: Replace this compatibility selection with either one TF publication per + # pose-targetable planning group or backend-level whole-robot TF publishing. + for group in self.groups_for_robot(robot_name): + if group.has_pose_target: + return group.id + return None diff --git a/dimos/manipulation/planning/groups/utils.py b/dimos/manipulation/planning/groups/utils.py new file mode 100644 index 0000000000..5c854ae60f --- /dev/null +++ b/dimos/manipulation/planning/groups/utils.py @@ -0,0 +1,136 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Shared helpers for planning-group selectors and joint-state projection.""" + +from collections.abc import Mapping, Sequence + +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.planning_identifiers import ( + assert_global_joint_names, + assert_local_joint_names, + is_global_joint_name, +) +from dimos.manipulation.planning.spec.models import ( + GlobalJointName, + LocalModelJointName, + PlanningGroupID, +) +from dimos.msgs.sensor_msgs.JointState import JointState + + +def planning_group_id_from_selector(selector: PlanningGroupID | PlanningGroup) -> PlanningGroupID: + """Return the planning-group ID represented by a selector.""" + if isinstance(selector, PlanningGroup): + return selector.id + return selector + + +def matching_global_joint_name( + positions_by_name: Mapping[str, float], local_joint_name: LocalModelJointName +) -> GlobalJointName | None: + """Find the unique global joint name ending with a local joint name.""" + suffix = f"/{local_joint_name}" + matches = [name for name in positions_by_name if name.endswith(suffix)] + if len(matches) == 1: + return matches[0] + return None + + +def filter_joint_state_to_selected_joints( + joint_state: JointState, + global_joint_names: Sequence[GlobalJointName], + local_joint_names: Sequence[LocalModelJointName] = (), +) -> JointState: + """Project a joint state to selected global joints. + + Values are looked up by global name first. When ``local_joint_names`` is + provided, each corresponding local name is used as a fallback. + """ + if local_joint_names and len(global_joint_names) != len(local_joint_names): + raise ValueError("Global and local selected joint lists must have the same length") + + positions_by_name = dict(zip(joint_state.name, joint_state.position, strict=True)) + selected_positions: list[float] = [] + missing: list[str] = [] + for index, global_name in enumerate(global_joint_names): + if global_name in positions_by_name: + selected_positions.append(float(positions_by_name[global_name])) + continue + if local_joint_names: + local_name = local_joint_names[index] + if local_name in positions_by_name: + selected_positions.append(float(positions_by_name[local_name])) + continue + missing.append(global_name) + + if missing: + raise ValueError(f"IK result is missing selected joints: {missing}") + + return JointState({"name": list(global_joint_names), "position": selected_positions}) + + +def joint_target_to_global_names( + group: PlanningGroup, + target: JointState, +) -> JointState: + """Convert a group joint target to global joint names in group order. + + Named targets may use either the public global planning names or the + robot-local model names used by legacy robot-scoped callers, but the two + namespaces must not be mixed in one target. + """ + if not target.name: + if len(target.position) != len(group.joint_names): + raise ValueError( + f"Target for '{group.id}' has {len(target.position)} positions, " + f"expected {len(group.joint_names)}" + ) + return JointState(name=list(group.joint_names), position=list(target.position)) + + if len(target.name) != len(target.position): + raise ValueError( + f"Target for '{group.id}' has {len(target.name)} names but " + f"{len(target.position)} positions" + ) + + target_names = list(target.name) + global_flags = [is_global_joint_name(name) for name in target_names] + if any(global_flags) and not all(global_flags): + raise ValueError( + f"Target for '{group.id}' mixes global and local joint names: {target_names}" + ) + + if all(global_flags): + assert_global_joint_names(target_names) + expected_names = group.joint_names + else: + assert_local_joint_names(target_names) + expected_names = group.local_joint_names + + positions_by_name = dict(zip(target_names, target.position, strict=True)) + global_positions: list[float] = [] + missing: list[str] = [] + for expected_name in expected_names: + if expected_name in positions_by_name: + global_positions.append(positions_by_name[expected_name]) + else: + missing.append(expected_name) + if missing: + raise ValueError(f"Target for '{group.id}' is missing joints: {missing}") + + extra = set(target_names) - set(expected_names) + if extra: + raise ValueError(f"Target for '{group.id}' has extra joints: {sorted(extra)}") + return JointState(name=list(group.joint_names), position=global_positions) diff --git a/dimos/manipulation/planning/kinematics/jacobian_ik.py b/dimos/manipulation/planning/kinematics/jacobian_ik.py index 7727b6fa0f..574d7d2bee 100644 --- a/dimos/manipulation/planning/kinematics/jacobian_ik.py +++ b/dimos/manipulation/planning/kinematics/jacobian_ik.py @@ -24,12 +24,20 @@ from __future__ import annotations +from collections.abc import Mapping, Sequence from typing import TYPE_CHECKING +import warnings import numpy as np +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection +from dimos.manipulation.planning.groups.utils import filter_joint_state_to_selected_joints from dimos.manipulation.planning.spec.enums import IKStatus -from dimos.manipulation.planning.spec.models import IKResult, WorldRobotID +from dimos.manipulation.planning.spec.models import ( + IKResult, + RobotName, + WorldRobotID, +) from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import ( check_singularity, @@ -53,7 +61,11 @@ class JacobianIK: - """Backend-agnostic Jacobian-based IK solver. + """Deprecated backend-agnostic Jacobian-based IK solver. + + Prefer PinkIK or DrakeOptimizationIK for new planning-group-aware code. + This class is retained as a compatibility/smoke-test backend and only + supports one directly pose-targeted planning group with no auxiliary groups. This class provides iterative and differential IK methods using only the standard WorldSpec interface. It works with any physics backend @@ -89,6 +101,11 @@ def __init__( max_iterations: Default maximum iterations for iterative IK singularity_threshold: Manipulability threshold for singularity detection """ + warnings.warn( + "JacobianIK is deprecated; use PinkIK or DrakeOptimizationIK for new code.", + DeprecationWarning, + stacklevel=2, + ) self._damping = damping self._max_iterations = max_iterations self._singularity_threshold = singularity_threshold @@ -185,6 +202,84 @@ def solve( f"IK failed after {max_attempts} attempts", ) + def solve_pose_targets( + self, + world: WorldSpec, + pose_targets: Mapping[PlanningGroup, PoseStamped], + auxiliary_groups: Sequence[PlanningGroup] = (), + seed: JointState | None = None, + position_tolerance: float = 0.001, + orientation_tolerance: float = 0.01, + check_collision: bool = True, + max_attempts: int = 10, + ) -> IKResult: + """Solve pose targets keyed by planning group with request-scoped auxiliaries. + + This backend currently supports exactly one directly pose-targeted planning group. + """ + if not pose_targets: + return _create_failure_result( + IKStatus.NO_SOLUTION, "At least one pose target is required" + ) + + pose_groups = tuple(pose_targets.keys()) + if len(pose_groups) != 1 or auxiliary_groups: + return _create_failure_result( + IKStatus.NO_SOLUTION, + "JacobianIK supports exactly one pose target and no auxiliary planning groups", + ) + + try: + selection = PlanningGroupSelection.from_groups(pose_groups + tuple(auxiliary_groups)) + robot_ids_by_name = _robot_ids_by_name(world, selection.robot_names) + except (KeyError, ValueError) as exc: + return _create_failure_result(IKStatus.NO_SOLUTION, str(exc)) + + target_group = pose_groups[0] + if not target_group.has_pose_target: + return _create_failure_result( + IKStatus.NO_SOLUTION, + f"Planning group '{target_group.id}' has no pose target frame", + ) + + robot_ids = {robot_ids_by_name[group.robot_name] for group in selection.groups} + if len(robot_ids) != 1: + return _create_failure_result( + IKStatus.NO_SOLUTION, + "JacobianIK does not support cross-robot pose IK", + ) + + robot_id = robot_ids_by_name[target_group.robot_name] + full_seed = seed + if full_seed is None: + with world.scratch_context() as ctx: + full_seed = world.get_joint_state(ctx, robot_id) + + target_pose = pose_targets[next(iter(pose_targets.keys()))] + result = self.solve( + world=world, + robot_id=robot_id, + target_pose=target_pose, + seed=full_seed, + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance, + check_collision=check_collision, + max_attempts=max_attempts, + ) + if not result.is_success() or result.joint_state is None: + return result + + selected_joint_names: list[str] = [] + for group in selection.groups: + selected_joint_names.extend(group.joint_names) + try: + result.joint_state = filter_joint_state_to_selected_joints( + result.joint_state, selected_joint_names + ) + except ValueError as exc: + return _create_failure_result(IKStatus.NO_SOLUTION, str(exc)) + return result + def solve_iterative( self, world: WorldSpec, @@ -433,3 +528,21 @@ def _create_failure_result( iterations=iterations, message=message, ) + + +def _robot_ids_by_name( + world: WorldSpec, robot_names: tuple[RobotName, ...] +) -> dict[RobotName, WorldRobotID]: + robot_ids_by_name: dict[RobotName, WorldRobotID] = {} + for robot_name in robot_names: + matches = [ + robot_id + for robot_id in world.get_robot_ids() + if world.get_robot_config(robot_id).name == robot_name + ] + if not matches: + raise KeyError(f"Robot '{robot_name}' not found") + if len(matches) > 1: + raise ValueError(f"Robot name '{robot_name}' is not unique in planning world") + robot_ids_by_name[robot_name] = matches[0] + return robot_ids_by_name diff --git a/dimos/manipulation/planning/kinematics/pink_ik.py b/dimos/manipulation/planning/kinematics/pink_ik.py index 1245e5aea4..bfb28b4639 100644 --- a/dimos/manipulation/planning/kinematics/pink_ik.py +++ b/dimos/manipulation/planning/kinematics/pink_ik.py @@ -16,6 +16,7 @@ from __future__ import annotations +from collections.abc import Mapping, Sequence from dataclasses import dataclass import importlib from pathlib import Path @@ -24,10 +25,17 @@ import numpy as np +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection +from dimos.manipulation.planning.groups.utils import matching_global_joint_name from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig +from dimos.manipulation.planning.planning_identifiers import make_global_joint_name from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import IKStatus -from dimos.manipulation.planning.spec.models import IKResult, WorldRobotID +from dimos.manipulation.planning.spec.models import ( + IKResult, + RobotName, + WorldRobotID, +) from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.kinematics_utils import compute_pose_error from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake @@ -74,6 +82,10 @@ class _PinkRobotContext: mapping: _JointMapping +class _CurrentStateRequiredError(ValueError): + """Raised when normalizing a seed requires the world's current state.""" + + class PinkIK: """Pink task/QP IK solver implementing the planning ``KinematicsSpec`` contract. @@ -164,6 +176,171 @@ def solve( return _failure(IKStatus.NO_SOLUTION, f"Pink IK failed after {max_attempts} attempts") + def solve_pose_targets( + self, + world: WorldSpec, + pose_targets: Mapping[PlanningGroup, PoseStamped], + auxiliary_groups: Sequence[PlanningGroup] = (), + seed: JointState | None = None, + position_tolerance: float = 0.001, + orientation_tolerance: float = 0.01, + check_collision: bool = True, + max_attempts: int = 10, + ) -> IKResult: + """Solve planning-group pose targets and return selected global joints.""" + if not pose_targets: + return _failure(IKStatus.NO_SOLUTION, "At least one pose target is required") + + pose_groups = tuple(pose_targets.keys()) + try: + selection = PlanningGroupSelection.from_groups(pose_groups + tuple(auxiliary_groups)) + robot_ids_by_name = _robot_ids_by_name(world, selection.robot_names) + except (KeyError, ValueError) as exc: + return _failure(IKStatus.NO_SOLUTION, str(exc)) + + groups_by_robot: dict[RobotName, list[PlanningGroup]] = {} + pose_groups_by_robot: dict[RobotName, list[PlanningGroup]] = {} + for group in selection.groups: + groups_by_robot.setdefault(group.robot_name, []).append(group) + for group in pose_groups: + if not group.has_pose_target or group.tip_link is None: + return _failure( + IKStatus.NO_SOLUTION, + f"Planning group '{group.id}' has no pose target frame", + ) + pose_groups_by_robot.setdefault(group.robot_name, []).append(group) + + selected_positions_by_name: dict[str, float] = {} + max_position_error = 0.0 + max_orientation_error = 0.0 + total_iterations = 0 + for robot_name, groups in groups_by_robot.items(): + robot_id = robot_ids_by_name[robot_name] + robot_pose_groups = pose_groups_by_robot.get(robot_name, []) + if robot_pose_groups: + result = self._solve_pose_targets_for_robot( + world=world, + robot_id=robot_id, + pose_targets={group: pose_targets[group] for group in robot_pose_groups}, + seed=_seed_for_robot_with_world_fallback(world, robot_id, seed), + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance, + check_collision=check_collision, + max_attempts=max_attempts, + ) + if not result.is_success() or result.joint_state is None: + return result + else: + result = IKResult( + status=IKStatus.SUCCESS, + joint_state=_seed_for_robot_with_world_fallback(world, robot_id, seed), + message="Auxiliary group retained seed state", + ) + joint_state = result.joint_state + if joint_state is None: + return _failure( + IKStatus.NO_SOLUTION, + f"Pink IK result for robot '{robot_name}' has no joint state", + ) + + max_position_error = max(max_position_error, result.position_error) + max_orientation_error = max(max_orientation_error, result.orientation_error) + total_iterations += result.iterations + local_positions = dict(zip(joint_state.name, joint_state.position, strict=True)) + for group in groups: + for global_name, local_name in zip( + group.joint_names, group.local_joint_names, strict=True + ): + if local_name not in local_positions: + return _failure( + IKStatus.NO_SOLUTION, + f"Pink IK result for robot '{robot_name}' is missing joint '{local_name}'", + ) + selected_positions_by_name[global_name] = float(local_positions[local_name]) + + selected_positions = [selected_positions_by_name[name] for name in selection.joint_names] + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState( + {"name": list(selection.joint_names), "position": selected_positions} + ), + position_error=max_position_error, + orientation_error=max_orientation_error, + iterations=total_iterations, + message="Pink IK target set solution found", + ) + + def _solve_pose_targets_for_robot( + self, + world: WorldSpec, + robot_id: WorldRobotID, + pose_targets: Mapping[PlanningGroup, PoseStamped], + seed: JointState, + position_tolerance: float, + orientation_tolerance: float, + check_collision: bool, + max_attempts: int, + ) -> IKResult: + """Solve one robot's one-or-more frame targets.""" + try: + contexts = [ + self._get_robot_context(world, robot_id, group.tip_link) + for group in pose_targets + if group.tip_link is not None + ] + except (FileNotFoundError, ImportError, ValueError) as exc: + return _failure(IKStatus.NO_SOLUTION, f"Pink IK model setup failed: {exc}") + + config = world.get_robot_config(robot_id) + lower_limits, upper_limits = world.get_joint_limits(robot_id) + target_models = [ + self._target_in_model_frame(config, pose) for pose in pose_targets.values() + ] + fallback_result: IKResult | None = None + + for attempt in range(max_attempts): + try: + q0 = self._initial_q(contexts[0], seed, lower_limits, upper_limits, attempt) + if len(contexts) == 1: + result = self._solve_single( + robot_context=contexts[0], + target_model=target_models[0], + seed_q=q0, + lower_limits=lower_limits, + upper_limits=upper_limits, + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance, + ) + else: + result = self._solve_multi_frame( + robot_contexts=contexts, + target_models=target_models, + seed_q=q0, + lower_limits=lower_limits, + upper_limits=upper_limits, + position_tolerance=position_tolerance, + orientation_tolerance=orientation_tolerance, + ) + except ValueError as exc: + return _failure(IKStatus.NO_SOLUTION, f"Pink IK mapping failed: {exc}") + except Exception as exc: + return _failure(IKStatus.NO_SOLUTION, f"Pink IK solver failed: {exc}") + + if not result.is_success() or result.joint_state is None: + if fallback_result is None: + fallback_result = result + continue + if check_collision and not world.check_config_collision_free( + robot_id, result.joint_state + ): + fallback_result = _collision_failure(result) + continue + return result + + if fallback_result is not None: + return fallback_result + return _failure(IKStatus.NO_SOLUTION, f"Pink IK failed after {max_attempts} attempts") + def _solve_single( self, robot_context: _PinkRobotContext, @@ -245,15 +422,116 @@ def _solve_single( message="Pink IK did not converge within the iteration budget", ) - def _get_robot_context(self, world: WorldSpec, robot_id: WorldRobotID) -> _PinkRobotContext: - cache_key = str(robot_id) - if cache_key not in self._robot_contexts: - self._robot_contexts[cache_key] = self._build_robot_context( - world.get_robot_config(robot_id) + def _solve_multi_frame( + self, + robot_contexts: Sequence[_PinkRobotContext], + target_models: Sequence[NDArray[np.float64]], + seed_q: NDArray[np.float64], + lower_limits: NDArray[np.float64], + upper_limits: NDArray[np.float64], + position_tolerance: float, + orientation_tolerance: float, + ) -> IKResult: + """Solve multiple frame tasks for one robot model.""" + pink = self._modules.pink + pinocchio = self._modules.pinocchio + primary_context = robot_contexts[0] + configuration = pink.Configuration( + primary_context.model, primary_context.data, seed_q.copy() + ) + + tasks: list[Any] = [] + for context, target_model in zip(robot_contexts, target_models, strict=True): + frame_task = pink.tasks.FrameTask( + context.frame_name, + position_cost=self.config.position_cost, + orientation_cost=self.config.orientation_cost, + lm_damping=self.config.lm_damping, + gain=self.config.gain, + ) + frame_task.set_target(_matrix_to_se3(pinocchio, target_model)) + tasks.append(frame_task) + + if self.config.posture_cost > 0.0: + posture_task = pink.tasks.PostureTask(cost=self.config.posture_cost) + posture_task.set_target_from_configuration(configuration) + tasks.append(posture_task) + + final_position_error = float("inf") + final_orientation_error = float("inf") + for iteration in range(self.config.max_iterations): + position_errors: list[float] = [] + orientation_errors: list[float] = [] + for context, target_model in zip(robot_contexts, target_models, strict=True): + current_pose = self._current_frame_matrix(context, configuration.q) + position_error, orientation_error = compute_pose_error(current_pose, target_model) + position_errors.append(position_error) + orientation_errors.append(orientation_error) + final_position_error = max(position_errors) + final_orientation_error = max(orientation_errors) + if ( + final_position_error <= position_tolerance + and final_orientation_error <= orientation_tolerance + ): + return _success( + primary_context.mapping.dimos_joint_names, + self._q_to_dimos_positions(primary_context, configuration.q), + final_position_error, + final_orientation_error, + iteration + 1, + ) + + velocity = pink.solve_ik( + configuration, + tasks, + self.config.dt, + solver=self.config.solver, + damping=self.config.damping, + safety_break=self.config.safety_break, ) + configuration.integrate_inplace(velocity, self.config.dt) + + joint_positions = self._q_to_dimos_positions(primary_context, configuration.q) + if not _within_limits(joint_positions, lower_limits, upper_limits): + return IKResult( + status=IKStatus.JOINT_LIMITS, + joint_state=None, + position_error=final_position_error, + orientation_error=final_orientation_error, + iterations=iteration + 1, + message="Pink IK candidate violates DimOS joint limits", + ) + + return IKResult( + status=IKStatus.NO_SOLUTION, + joint_state=None, + position_error=final_position_error, + orientation_error=final_orientation_error, + iterations=self.config.max_iterations, + message="Pink IK did not converge within the iteration budget", + ) + + def _get_robot_context( + self, world: WorldSpec, robot_id: WorldRobotID, frame_name: str | None = None + ) -> _PinkRobotContext: + config = world.get_robot_config(robot_id) + target_frame = frame_name or config.end_effector_link + if target_frame is None: + raise ValueError(f"Robot '{robot_id}' has no end-effector frame configured") + cache_key = f"{robot_id}:{target_frame}" + if ( + cache_key not in self._robot_contexts + and frame_name is None + and str(robot_id) in self._robot_contexts + ): + return self._robot_contexts[str(robot_id)] + if cache_key not in self._robot_contexts: + self._robot_contexts[cache_key] = self._build_robot_context(config, target_frame) return self._robot_contexts[cache_key] - def _build_robot_context(self, config: RobotModelConfig) -> _PinkRobotContext: + def _build_robot_context( + self, config: RobotModelConfig, frame_name: str | None = None + ) -> _PinkRobotContext: pinocchio = self._modules.pinocchio model_path = Path(config.model_path).resolve() if not model_path.exists(): @@ -267,17 +545,24 @@ def _build_robot_context(self, config: RobotModelConfig) -> _PinkRobotContext: package_paths=config.package_paths, xacro_args=config.xacro_args, convert_meshes=config.auto_convert_meshes, + strip_world_joint_child_link=config.base_link + if config.strip_model_world_joint + else None, ) model = pinocchio.buildModelFromUrdf(str(prepared_path)) + model = _lock_uncontrolled_model_joints(pinocchio, model, config) data = model.createData() - frame_id = _get_frame_id(model, config.end_effector_link) + target_frame = frame_name or config.end_effector_link + if target_frame is None: + raise ValueError("Robot model has no end-effector frame configured") + frame_id = _get_frame_id(model, target_frame) mapping = _build_joint_mapping(model, config) return _PinkRobotContext( model=model, data=data, frame_id=frame_id, - frame_name=config.end_effector_link, + frame_name=target_frame, mapping=mapping, ) @@ -370,7 +655,7 @@ def _build_joint_mapping(model: Any, config: RobotModelConfig) -> _JointMapping: model_joint_names: list[str] = [] for dimos_name in config.joint_names: - model_joint_name = config.get_urdf_joint_name(dimos_name) + model_joint_name = dimos_name joint_id = _get_joint_id(model, model_joint_name) joint = model.joints[joint_id] nq = int(getattr(joint, "nq", 1)) @@ -389,6 +674,30 @@ def _build_joint_mapping(model: Any, config: RobotModelConfig) -> _JointMapping: ) +def _lock_uncontrolled_model_joints( + pinocchio: ModuleType, model: Any, config: RobotModelConfig +) -> Any: + """Return a Pinocchio model reduced to the joints controlled by config.""" + controlled_joint_names = set(config.joint_names) + lock_joint_ids: list[int] = [] + for joint_id, model_joint_name in enumerate(model.names): + if joint_id == 0 or model_joint_name in controlled_joint_names: + continue + joint = model.joints[joint_id] + if int(getattr(joint, "nq", 1)) > 0: + lock_joint_ids.append(joint_id) + + if not lock_joint_ids: + return model + + logger.debug( + "Reducing Pink IK model '%s' by locking uncontrolled joints: %s", + config.name, + [model.names[joint_id] for joint_id in lock_joint_ids], + ) + return pinocchio.buildReducedModel(model, lock_joint_ids, pinocchio.neutral(model)) + + def _get_joint_id(model: Any, joint_name: str) -> int: if hasattr(model, "existJointName") and not model.existJointName(joint_name): raise ValueError(_missing_joint_message(model, joint_name)) @@ -429,6 +738,10 @@ def _seed_positions_for_mapping(seed: JointState, mapping: _JointMapping) -> NDA values.append(float(positions_by_name[dimos_name])) elif model_name in positions_by_name: values.append(float(positions_by_name[model_name])) + elif ( + global_name := matching_global_joint_name(positions_by_name, dimos_name) + ) is not None: + values.append(float(positions_by_name[global_name])) else: raise ValueError(f"Seed is missing joint '{dimos_name}' (URDF name '{model_name}')") return np.array(values, dtype=np.float64) @@ -465,7 +778,7 @@ def _success( ) -> IKResult: return IKResult( status=IKStatus.SUCCESS, - joint_state=JointState(name=joint_names, position=joint_positions.tolist()), + joint_state=JointState({"name": joint_names, "position": joint_positions.tolist()}), position_error=position_error, orientation_error=orientation_error, iterations=iterations, @@ -486,3 +799,85 @@ def _collision_failure(result: IKResult) -> IKResult: iterations=result.iterations, message="Pink IK solution rejected by collision check", ) + + +def _seed_for_robot_config( + config: RobotModelConfig, + seed: JointState | None, + current_state: JointState | None = None, +) -> JointState: + """Return a full local seed state for one robot from local/global seed input.""" + if seed is None: + if current_state is None: + raise _CurrentStateRequiredError("Current joint state is required when seed is absent") + return JointState(current_state) + if not seed.name: + if len(seed.position) == len(config.joint_names): + return JointState({"name": list(config.joint_names), "position": list(seed.position)}) + raise ValueError( + f"Seed has {len(seed.position)} positions for robot '{config.name}', " + f"expected {len(config.joint_names)}" + ) + if len(seed.name) != len(seed.position): + raise ValueError(f"Seed has {len(seed.name)} names but {len(seed.position)} positions") + seed_by_name = dict(zip(seed.name, seed.position, strict=True)) + positions: list[float] = [] + missing_local_names: list[str] = [] + for local_name in config.joint_names: + global_name = make_global_joint_name(config.name, local_name) + if local_name in seed_by_name: + positions.append(float(seed_by_name[local_name])) + elif global_name in seed_by_name: + positions.append(float(seed_by_name[global_name])) + else: + positions.append(0.0) + missing_local_names.append(local_name) + if missing_local_names: + if current_state is None: + missing = ", ".join(repr(name) for name in missing_local_names) + raise _CurrentStateRequiredError( + f"Current joint state is required for missing joints: {missing}" + ) + current = current_state + current_by_name = dict(zip(current.name, current.position, strict=True)) + for index, local_name in enumerate(config.joint_names): + if local_name not in missing_local_names: + continue + if local_name not in current_by_name: + raise ValueError(f"Seed/current state is missing joint '{local_name}'") + positions[index] = float(current_by_name[local_name]) + return JointState({"name": list(config.joint_names), "position": positions}) + + +def _seed_for_robot_with_world_fallback( + world: WorldSpec, robot_id: WorldRobotID, seed: JointState | None +) -> JointState: + """Normalize a robot seed, reading world state only when the seed is incomplete.""" + config = world.get_robot_config(robot_id) + try: + return _seed_for_robot_config(config, seed) + except _CurrentStateRequiredError: + with world.scratch_context() as ctx: + current = world.get_joint_state(ctx, robot_id) + return _seed_for_robot_config(config, seed, current) + + +def _robot_ids_by_name( + world: WorldSpec, robot_names: tuple[RobotName, ...] +) -> dict[RobotName, WorldRobotID]: + robot_ids_by_name: dict[RobotName, WorldRobotID] = {} + for robot_name in robot_names: + matches = [ + robot_id + for robot_id in world.get_robot_ids() + if world.get_robot_config(robot_id).name == robot_name + ] + if not matches: + raise KeyError(f"Robot '{robot_name}' not found") + if len(matches) > 1: + raise ValueError(f"Robot name '{robot_name}' is not unique in planning world") + robot_ids_by_name[robot_name] = matches[0] + return robot_ids_by_name + + +__all__ = ["PinkIK", "PinkIKConfig", "PinkIKDependencyError"] diff --git a/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py new file mode 100644 index 0000000000..a850b12aab --- /dev/null +++ b/dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py @@ -0,0 +1,138 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for Jacobian IK selected planning group result contracts.""" + +from __future__ import annotations + +from collections.abc import Mapping +from pathlib import Path +from typing import cast + +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.kinematics.jacobian_ik import JacobianIK +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.spec.enums import IKStatus +from dimos.manipulation.planning.spec.models import IKResult +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState + + +def _pose() -> PoseStamped: + return PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]) + + +def _joint_state(names: list[str], positions: list[float]) -> JointState: + return JointState({"name": names, "position": positions}) + + +def _group( + group_id: str, joint_names: tuple[str, ...], tip_link: str | None = "tool0" +) -> PlanningGroup: + return PlanningGroup( + id=group_id, + robot_name="arm", + group_name=group_id.split("/", maxsplit=1)[1], + joint_names=joint_names, + local_joint_names=tuple(name.split("/", maxsplit=1)[1] for name in joint_names), + base_link="base_link", + tip_link=tip_link, + ) + + +class _IKWorld: + def __init__(self, groups: Mapping[str, PlanningGroup]) -> None: + self._groups = groups + self._robot_configs = { + "robot_1": RobotModelConfig( + name="arm", + model_path=Path("robot.urdf"), + base_pose=_pose(), + joint_names=["joint1", "joint2", "gripper"], + end_effector_link="tool0", + ) + } + + def get_robot_ids(self) -> list[str]: + return list(self._robot_configs) + + def get_robot_config(self, robot_id: str) -> RobotModelConfig: + return self._robot_configs[robot_id] + + +class _SuccessfulIK(JacobianIK): + def solve( + self, + world: WorldSpec, + robot_id: str, + target_pose: PoseStamped, + seed: JointState | None = None, + position_tolerance: float = 0.001, + orientation_tolerance: float = 0.01, + check_collision: bool = True, + max_attempts: int = 10, + ) -> IKResult: + return IKResult( + status=IKStatus.SUCCESS, + joint_state=_joint_state( + ["arm/joint1", "arm/joint2", "arm/gripper", "arm/unrelated"], + [0.1, 0.2, 0.3, 0.4], + ), + ) + + +def test_solve_pose_targets_filters_result_to_single_group_joints() -> None: + world = _IKWorld( + { + "arm/arm": _group("arm/arm", ("arm/joint1", "arm/joint2")), + } + ) + + result = _SuccessfulIK().solve_pose_targets( + world=cast("WorldSpec", world), + pose_targets={world._groups["arm/arm"]: _pose()}, + seed=_joint_state(["arm/joint1", "arm/joint2", "arm/gripper"], [0.0, 0.0, 0.0]), + ) + + assert result.status == IKStatus.SUCCESS + assert result.joint_state is not None + assert result.joint_state.name == ["arm/joint1", "arm/joint2"] + assert result.joint_state.position == [0.1, 0.2] + + +def test_solve_pose_targets_rejects_auxiliary_groups() -> None: + world = _IKWorld({"arm/arm": _group("arm/arm", ("arm/joint1", "arm/joint2"))}) + + result = _SuccessfulIK().solve_pose_targets( + world=cast("WorldSpec", world), + pose_targets={world._groups["arm/arm"]: _pose()}, + auxiliary_groups=[_group("arm/gripper", ("arm/gripper",))], + seed=_joint_state(["arm/joint1", "arm/joint2"], [0.0, 0.0]), + ) + + assert result.status == IKStatus.NO_SOLUTION + assert "no auxiliary planning groups" in result.message + + +def test_solve_pose_targets_rejects_group_without_pose_target_frame() -> None: + world = _IKWorld({"arm/gripper": _group("arm/gripper", ("arm/gripper",), tip_link=None)}) + + result = JacobianIK().solve_pose_targets( + world=cast("WorldSpec", world), + pose_targets={world._groups["arm/gripper"]: _pose()}, + ) + + assert result.status == IKStatus.NO_SOLUTION + assert "no pose target frame" in result.message diff --git a/dimos/manipulation/planning/kinematics/test_pink_ik.py b/dimos/manipulation/planning/kinematics/test_pink_ik.py index 2c2fd863e2..fdc4728093 100644 --- a/dimos/manipulation/planning/kinematics/test_pink_ik.py +++ b/dimos/manipulation/planning/kinematics/test_pink_ik.py @@ -25,14 +25,17 @@ import pytest from dimos.manipulation.planning.factory import create_kinematics +from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.kinematics.pink_ik import ( PinkIK, PinkIKConfig, PinkIKDependencyError, _build_joint_mapping, + _lock_uncontrolled_model_joints, _PinkModules, _PinkRobotContext, + _seed_for_robot_config, _seed_positions_for_mapping, ) from dimos.manipulation.planning.spec.config import RobotModelConfig @@ -73,9 +76,9 @@ class _FakeModel: def __init__(self) -> None: self.names = ["universe", "joint_b", "joint_a", "joint_c"] self.joints = [SimpleNamespace(idx_q=-1, nq=0), _FakeJoint(0), _FakeJoint(1), _FakeJoint(2)] - self.frames = [_FakeFrame("tool")] + self.frames = [_FakeFrame("tool"), _FakeFrame("wrist_tool")] self._joint_ids = {"joint_b": 1, "joint_a": 2, "joint_c": 3} - self._frame_ids = {"tool": 0} + self._frame_ids = {"tool": 0, "wrist_tool": 1} def createData(self) -> _FakeData: return _FakeData() @@ -172,6 +175,15 @@ def _robot_config() -> RobotModelConfig: ) +def _pose_stamped(x: float, y: float, z: float, yaw: float = 0.0) -> PoseStamped: + half_yaw = yaw / 2.0 + return PoseStamped( + frame_id="world", + position=Vector3(x, y, z), + orientation=Quaternion(0.0, 0.0, float(np.sin(half_yaw)), float(np.cos(half_yaw))), + ) + + class _TestPinkIK(PinkIK): def __init__(self, converge: bool = True) -> None: self.config = PinkIKConfig(max_iterations=3) @@ -201,6 +213,29 @@ class _FakeWorld: def __init__(self, collision_free: bool = True) -> None: self.config = _robot_config() self.collision_free = collision_free + self.groups = { + "arm/wrist": PlanningGroup( + id="arm/wrist", + robot_name="arm", + group_name="wrist", + joint_names=("arm/joint_a", "arm/joint_b"), + local_joint_names=("joint_a", "joint_b"), + base_link="base", + tip_link="wrist_tool", + ), + "arm/gripper": PlanningGroup( + id="arm/gripper", + robot_name="arm", + group_name="gripper", + joint_names=("arm/joint_c",), + local_joint_names=("joint_c",), + base_link="base", + tip_link=None, + ), + } + + def get_robot_ids(self) -> list[str]: + return ["robot"] def get_robot_config(self, robot_id: str) -> RobotModelConfig: return self.config @@ -221,6 +256,47 @@ def check_config_collision_free(self, robot_id: str, joint_state: JointState) -> return self.collision_free +class _FakeMultiRobotWorld: + is_finalized = True + + def __init__(self) -> None: + self.configs = { + "left_robot": RobotModelConfig( + name="left", + model_path=Path("/tmp/left.urdf"), + base_pose=PoseStamped( + position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + ), + joint_names=["joint_a", "joint_b"], + end_effector_link="tool", + base_link="base", + ), + "right_robot": RobotModelConfig( + name="right", + model_path=Path("/tmp/right.urdf"), + base_pose=PoseStamped( + position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0) + ), + joint_names=["joint_c"], + end_effector_link="tool", + base_link="base", + ), + } + + def get_robot_ids(self) -> list[str]: + return list(self.configs) + + def get_robot_config(self, robot_id: str) -> RobotModelConfig: + return self.configs[robot_id] + + def scratch_context(self) -> nullcontext[None]: + return nullcontext(None) + + def get_joint_state(self, ctx: object, robot_id: str) -> JointState: + config = self.get_robot_config(robot_id) + return JointState(name=list(config.joint_names), position=[0.0] * len(config.joint_names)) + + def test_create_kinematics_pink_missing_dependency_is_actionable( monkeypatch: pytest.MonkeyPatch, ) -> None: @@ -302,6 +378,15 @@ def test_joint_order_mapping_uses_names_not_positions() -> None: assert _seed_positions_for_mapping(seed, mapping).tolist() == [10.0, 20.0, 30.0] +def test_seed_for_robot_config_uses_complete_global_seed_without_world() -> None: + seed = JointState(name=["arm/joint_a", "arm/joint_b", "arm/joint_c"], position=[1.0, 2.0, 3.0]) + + result = _seed_for_robot_config(_robot_config(), seed) + + assert result.name == ["joint_a", "joint_b", "joint_c"] + assert result.position == [1.0, 2.0, 3.0] + + def test_mapping_failure_for_missing_joint() -> None: config = _robot_config() config.joint_names = ["joint_a", "missing", "joint_c"] @@ -310,6 +395,31 @@ def test_mapping_failure_for_missing_joint() -> None: _build_joint_mapping(_FakeModel(), config) +def test_uncontrolled_urdf_joints_are_locked_out_of_pink_model() -> None: + pinocchio = ModuleType("pinocchio") + model = _FakeModel() + model.names.append("gripper_joint") + model.joints.append(_FakeJoint(3)) + reduced_model = _FakeModel() + seen_locked_joint_ids: list[list[int]] = [] + + def build_reduced_model( + input_model: _FakeModel, locked_joint_ids: list[int], reference: np.ndarray + ) -> _FakeModel: + assert input_model is model + np.testing.assert_allclose(reference, np.zeros(model.nq)) + seen_locked_joint_ids.append(list(locked_joint_ids)) + return reduced_model + + pinocchio.neutral = lambda input_model: np.zeros(input_model.nq) # type: ignore[attr-defined] + pinocchio.buildReducedModel = build_reduced_model # type: ignore[attr-defined] + + result = _lock_uncontrolled_model_joints(pinocchio, model, _robot_config()) + + assert result is reduced_model + assert seen_locked_joint_ids == [[4]] + + def test_solve_single_returns_successful_ik_result() -> None: ik = _pink_ik(converge=True) target = np.eye(4) @@ -370,6 +480,239 @@ def test_solve_rejects_collision_candidate() -> None: assert result.joint_state is None +def test_solve_pose_targets_returns_selected_resolved_joints_and_group_tip( + monkeypatch: pytest.MonkeyPatch, +) -> None: + ik = _pink_ik(converge=True) + context = _context() + context.frame_name = "wrist_tool" + context.frame_id = 1 + ik._robot_contexts = {"robot:wrist_tool": context} + seen_frame_names: list[str] = [] + + def fake_solve_single(**kwargs: object) -> IKResult: + robot_context = cast("_PinkRobotContext", kwargs["robot_context"]) + seen_frame_names.append(robot_context.frame_name) + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState( + {"name": ["joint_a", "joint_b", "joint_c"], "position": [0.1, 0.2, 0.3]} + ), + position_error=0.0, + orientation_error=0.0, + iterations=1, + ) + + monkeypatch.setattr(ik, "_solve_single", fake_solve_single) + + world = _FakeWorld(collision_free=True) + result = ik.solve_pose_targets( + world=cast("Any", world), + pose_targets={ + world.groups["arm/wrist"]: PoseStamped( + position=Vector3(0.1, 0.0, 0.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ) + }, + auxiliary_groups=[world.groups["arm/gripper"]], + seed=JointState( + {"name": ["arm/joint_a", "arm/joint_b", "arm/joint_c"], "position": [0.0, 0.0, 0.0]} + ), + check_collision=False, + max_attempts=1, + ) + + assert seen_frame_names == ["wrist_tool"] + assert result.status == IKStatus.SUCCESS + assert result.joint_state is not None + assert result.joint_state.name == ["arm/joint_a", "arm/joint_b", "arm/joint_c"] + assert result.joint_state.position == [0.1, 0.2, 0.3] + + +def test_solve_pose_targets_same_robot_uses_one_multi_frame_solve( + monkeypatch: pytest.MonkeyPatch, +) -> None: + ik = _pink_ik(converge=True) + wrist_context = _context() + wrist_context.frame_name = "wrist_tool" + wrist_context.frame_id = 1 + tool_context = _context() + ik._robot_contexts = {"robot:wrist_tool": wrist_context, "robot:tool": tool_context} + world = _FakeWorld(collision_free=True) + tool_group = PlanningGroup( + id="arm/tool", + robot_name="arm", + group_name="tool", + joint_names=("arm/joint_c",), + local_joint_names=("joint_c",), + base_link="base", + tip_link="tool", + ) + seen_frames: list[list[str]] = [] + + def fake_solve_multi_frame(**kwargs: object) -> IKResult: + contexts = cast("list[_PinkRobotContext]", kwargs["robot_contexts"]) + seen_frames.append([context.frame_name for context in contexts]) + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState( + {"name": ["joint_a", "joint_b", "joint_c"], "position": [0.1, 0.2, 0.3]} + ), + position_error=0.0, + orientation_error=0.0, + iterations=2, + ) + + monkeypatch.setattr(ik, "_solve_multi_frame", fake_solve_multi_frame) + + result = ik.solve_pose_targets( + world=cast("Any", world), + pose_targets={ + world.groups["arm/wrist"]: PoseStamped( + position=Vector3(0.1, 0.0, 0.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + tool_group: PoseStamped( + position=Vector3(0.2, 0.0, 0.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + }, + seed=JointState( + {"name": ["arm/joint_a", "arm/joint_b", "arm/joint_c"], "position": [0.0, 0.0, 0.0]} + ), + check_collision=False, + max_attempts=1, + ) + + assert seen_frames == [["wrist_tool", "tool"]] + assert result.status == IKStatus.SUCCESS + assert result.joint_state is not None + assert result.joint_state.name == ["arm/joint_a", "arm/joint_b", "arm/joint_c"] + assert result.joint_state.position == [0.1, 0.2, 0.3] + + +def test_target_in_model_frame_converts_world_pose_through_robot_base() -> None: + ik = _pink_ik(converge=True) + config = _robot_config() + config.base_pose = _pose_stamped(1.0, 2.0, 0.0, yaw=np.pi / 2.0) + target_world = _pose_stamped(0.8, 2.1, 0.3, yaw=np.pi / 2.0) + + target_model = ik._target_in_model_frame(config, target_world) + + np.testing.assert_allclose(target_model[:3, 3], [0.1, 0.2, 0.3], atol=1e-12) + np.testing.assert_allclose(target_model[:3, :3], np.eye(3), atol=1e-12) + + +def test_solve_pose_targets_passes_world_target_to_solver_in_model_frame( + monkeypatch: pytest.MonkeyPatch, +) -> None: + ik = _pink_ik(converge=True) + context = _context() + context.frame_name = "wrist_tool" + context.frame_id = 1 + ik._robot_contexts = {"robot:wrist_tool": context} + world = _FakeWorld(collision_free=True) + world.config.base_pose = _pose_stamped(1.0, 2.0, 0.0, yaw=np.pi / 2.0) + seen_target_models: list[np.ndarray] = [] + + def fake_solve_single(**kwargs: object) -> IKResult: + target_model = cast("np.ndarray", kwargs["target_model"]) + seen_target_models.append(target_model) + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState( + name=["joint_a", "joint_b", "joint_c"], position=[0.1, 0.2, 0.3] + ), + position_error=0.0, + orientation_error=0.0, + iterations=1, + ) + + monkeypatch.setattr(ik, "_solve_single", fake_solve_single) + + result = ik.solve_pose_targets( + world=cast("Any", world), + pose_targets={world.groups["arm/wrist"]: _pose_stamped(0.8, 2.1, 0.3, yaw=np.pi / 2.0)}, + seed=JointState( + {"name": ["arm/joint_a", "arm/joint_b", "arm/joint_c"], "position": [0.0, 0.0, 0.0]} + ), + check_collision=False, + max_attempts=1, + ) + + assert result.status == IKStatus.SUCCESS + assert len(seen_target_models) == 1 + np.testing.assert_allclose(seen_target_models[0][:3, 3], [0.1, 0.2, 0.3], atol=1e-12) + np.testing.assert_allclose(seen_target_models[0][:3, :3], np.eye(3), atol=1e-12) + + +def test_solve_pose_targets_cross_robot_combines_global_joint_names( + monkeypatch: pytest.MonkeyPatch, +) -> None: + ik = _pink_ik(converge=True) + world = _FakeMultiRobotWorld() + left_group = PlanningGroup( + id="left/arm", + robot_name="left", + group_name="arm", + joint_names=("left/joint_a",), + local_joint_names=("joint_a",), + base_link="base", + tip_link="tool", + ) + right_group = PlanningGroup( + id="right/arm", + robot_name="right", + group_name="arm", + joint_names=("right/joint_c",), + local_joint_names=("joint_c",), + base_link="base", + tip_link="tool", + ) + seen_robot_ids: list[str] = [] + + def fake_solve_pose_targets_for_robot(**kwargs: object) -> IKResult: + robot_id = str(kwargs["robot_id"]) + seen_robot_ids.append(robot_id) + if robot_id == "left_robot": + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState(name=["joint_a", "joint_b"], position=[1.0, 9.0]), + ) + return IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState(name=["joint_c"], position=[2.0]), + ) + + monkeypatch.setattr(ik, "_solve_pose_targets_for_robot", fake_solve_pose_targets_for_robot) + + result = ik.solve_pose_targets( + world=cast("Any", world), + pose_targets={ + left_group: PoseStamped( + position=Vector3(0.1, 0.0, 0.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + right_group: PoseStamped( + position=Vector3(0.2, 0.0, 0.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + }, + seed=JointState( + name=["left/joint_a", "left/joint_b", "right/joint_c"], + position=[0.0, 0.0, 0.0], + ), + check_collision=False, + max_attempts=1, + ) + + assert seen_robot_ids == ["left_robot", "right_robot"] + assert result.status == IKStatus.SUCCESS + assert result.joint_state is not None + assert result.joint_state.name == ["left/joint_a", "right/joint_c"] + assert result.joint_state.position == [1.0, 2.0] + + def test_solve_retries_after_joint_limit_failure(monkeypatch: pytest.MonkeyPatch) -> None: ik = _pink_ik(converge=True) context = _context() diff --git a/dimos/manipulation/planning/monitor/robot_state_monitor.py b/dimos/manipulation/planning/monitor/robot_state_monitor.py index 49c6b56366..39492a80ac 100644 --- a/dimos/manipulation/planning/monitor/robot_state_monitor.py +++ b/dimos/manipulation/planning/monitor/robot_state_monitor.py @@ -69,7 +69,6 @@ def __init__( lock: threading.RLock, robot_id: str, joint_names: list[str], - joint_name_mapping: dict[str, str] | None = None, timeout: float = 1.0, ) -> None: """Create a world state monitor. @@ -78,10 +77,7 @@ def __init__( world: WorldSpec instance to sync state to lock: Shared lock for thread-safe access robot_id: ID of the robot to monitor - joint_names: Ordered list of joint names for this robot (URDF names) - joint_name_mapping: Maps coordinator joint names to URDF joint names. - Example: {"left/joint1": "joint1"} means messages with "left/joint1" - will be mapped to URDF "joint1". If None, names must match exactly. + joint_names: Ordered list of local model joint names for this robot timeout: Timeout for waiting for initial state (seconds) """ self._world = world @@ -90,11 +86,6 @@ def __init__( self._joint_names = joint_names self._timeout = timeout - # Joint name mapping: coordinator name -> URDF name - self._joint_name_mapping = joint_name_mapping or {} - # Build reverse mapping: URDF name -> coordinator name - self._reverse_mapping = {v: k for k, v in self._joint_name_mapping.items()} - # Latest state self._latest_positions: NDArray[np.float64] | None = None self._latest_velocities: NDArray[np.float64] | None = None @@ -190,30 +181,19 @@ def on_joint_state(self, msg: JointState) -> None: def _extract_positions(self, msg: JointState) -> NDArray[np.float64] | None: """Extract positions for our joints from JointState message. - Handles joint name translation from coordinator namespace to URDF namespace. - If joint_name_mapping is set, message names are looked up via the reverse mapping. - Args: - msg: JointState message (may use coordinator joint names) + msg: Robot-scoped JointState message with local model joint names Returns: Array of joint positions or None if any joint is missing """ - # Build name->index map from message (coordinator names) name_to_idx = {name: i for i, name in enumerate(msg.name)} positions = [] - for urdf_joint_name in self._joint_names: - # Try direct match first (when no mapping or names already match) - if urdf_joint_name in name_to_idx: - idx = name_to_idx[urdf_joint_name] - else: - # Try reverse mapping: URDF name -> coordinator name -> msg index - orch_name = self._reverse_mapping.get(urdf_joint_name) - if orch_name is None or orch_name not in name_to_idx: - return None # Missing joint - idx = name_to_idx[orch_name] - + for local_joint_name in self._joint_names: + idx = name_to_idx.get(local_joint_name) + if idx is None: + return None if idx >= len(msg.position): return None # Position not available positions.append(msg.position[idx]) @@ -223,7 +203,7 @@ def _extract_positions(self, msg: JointState) -> NDArray[np.float64] | None: def _extract_velocities(self, msg: JointState) -> NDArray[np.float64] | None: """Extract velocities for our joints. - Uses same name translation as _extract_positions. + Uses the same local-name lookup as _extract_positions. """ if not msg.velocity or len(msg.velocity) == 0: return None @@ -231,17 +211,10 @@ def _extract_velocities(self, msg: JointState) -> NDArray[np.float64] | None: name_to_idx = {name: i for i, name in enumerate(msg.name)} velocities = [] - for urdf_joint_name in self._joint_names: - # Try direct match first - if urdf_joint_name in name_to_idx: - idx = name_to_idx[urdf_joint_name] - else: - # Try reverse mapping - orch_name = self._reverse_mapping.get(urdf_joint_name) - if orch_name is None or orch_name not in name_to_idx: - return None - idx = name_to_idx[orch_name] - + for local_joint_name in self._joint_names: + idx = name_to_idx.get(local_joint_name) + if idx is None: + return None if idx >= len(msg.velocity): return None velocities.append(msg.velocity[idx]) diff --git a/dimos/manipulation/planning/monitor/test_world_monitor.py b/dimos/manipulation/planning/monitor/test_world_monitor.py index d46eb73a6c..4e12c5dd57 100644 --- a/dimos/manipulation/planning/monitor/test_world_monitor.py +++ b/dimos/manipulation/planning/monitor/test_world_monitor.py @@ -14,13 +14,18 @@ from __future__ import annotations +from collections.abc import Sequence from pathlib import Path from typing import Any from dimos.manipulation.planning import factory as planning_factory from dimos.manipulation.planning.monitor import world_monitor as world_monitor_module from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.manipulation.planning.spec.models import PlanningSceneInfo +from dimos.manipulation.planning.spec.models import ( + GeneratedPlan, + PlanningGroupID, + PlanningSceneInfo, +) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -110,13 +115,13 @@ def initialize_scene(self, scene: PlanningSceneInfo) -> None: def publish_visualization(self, ctx=None) -> None: return None - def show_preview(self, robot_id) -> None: + def show_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: return None - def hide_preview(self, robot_id) -> None: + def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: return None - def animate_path(self, robot_id, path, duration: float = 3.0) -> None: + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: return None def close(self) -> None: @@ -136,13 +141,13 @@ def initialize_scene(self, scene: PlanningSceneInfo) -> None: def publish_visualization(self, ctx=None) -> None: return None - def show_preview(self, robot_id) -> None: - self.calls.append(("show_preview", robot_id)) + def show_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + self.calls.append(("show_preview", tuple(group_ids))) - def hide_preview(self, robot_id) -> None: - self.calls.append(("hide_preview", robot_id)) + def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + self.calls.append(("hide_preview", tuple(group_ids))) - def animate_path(self, robot_id, path, duration: float = 3.0) -> None: + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: return None def close(self) -> None: diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index 5e12568874..cf0c605a3c 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -21,16 +21,18 @@ from typing import TYPE_CHECKING, Any from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry from dimos.manipulation.planning.monitor.robot_state_monitor import RobotStateMonitor from dimos.manipulation.planning.monitor.world_obstacle_monitor import WorldObstacleMonitor from dimos.manipulation.planning.spec.models import PlanningSceneInfo from dimos.manipulation.planning.spec.protocols import VisualizationSpec, WorldSpec from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from collections.abc import Generator + from collections.abc import Generator, Sequence import numpy as np from numpy.typing import NDArray @@ -38,8 +40,10 @@ from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( CollisionObjectMessage, + GeneratedPlan, JointPath, Obstacle, + PlanningGroupID, WorldRobotID, ) from dimos.msgs.vision_msgs.Detection3D import Detection3D @@ -60,6 +64,8 @@ def __init__( self._visualization = visualization self._lock = threading.RLock() self._robot_joints: dict[WorldRobotID, list[str]] = {} + self._robot_ids_by_name: dict[str, WorldRobotID] = {} + self._planning_groups = PlanningGroupRegistry() self._robot_configs: dict[WorldRobotID, RobotModelConfig] = {} self._state_monitors: dict[WorldRobotID, RobotStateMonitor] = {} self._obstacle_monitor: WorldObstacleMonitor | None = None @@ -74,6 +80,10 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: with self._lock: robot_id = self._world.add_robot(config) self._robot_joints[robot_id] = config.joint_names + if config.name in self._robot_ids_by_name: + raise ValueError(f"Robot name '{config.name}' is already registered") + self._robot_ids_by_name[config.name] = robot_id + self._planning_groups.add_robot(config) self._robot_configs[robot_id] = config logger.info(f"Added robot '{config.name}' as '{robot_id}'") return robot_id @@ -90,6 +100,11 @@ def sync_visualization_scene(self) -> None: return visualization.initialize_scene(self.planning_scene_info()) + @property + def planning_groups(self) -> PlanningGroupRegistry: + """Backend-independent planning-group registry for added robots.""" + return self._planning_groups + def get_robot_ids(self) -> list[WorldRobotID]: """Get all robot IDs.""" with self._lock: @@ -130,7 +145,6 @@ def start_state_monitor( self, robot_id: WorldRobotID, joint_names: list[str] | None = None, - joint_name_mapping: dict[str, str] | None = None, ) -> None: """Start monitoring joint states. Uses config defaults if args are None.""" with self._lock: @@ -148,16 +162,11 @@ def start_state_monitor( else: joint_names = config.joint_names - # Get joint name mapping from config if not provided - if joint_name_mapping is None and config.joint_name_mapping: - joint_name_mapping = config.joint_name_mapping - monitor = RobotStateMonitor( world=self._world, lock=self._lock, robot_id=robot_id, joint_names=joint_names, - joint_name_mapping=joint_name_mapping, ) monitor.start() self._state_monitors[robot_id] = monitor @@ -366,16 +375,24 @@ def get_min_distance(self, robot_id: WorldRobotID) -> float: def get_ee_pose( self, robot_id: WorldRobotID, joint_state: JointState | None = None ) -> PoseStamped: - """Get end-effector pose. Uses current state if joint_state is None.""" + """Get end-effector pose for the robot's unique pose-targetable group.""" + return self.get_group_pose( + self._unique_pose_group_id_for_robot(robot_id), + joint_state=joint_state, + ) + + def get_group_pose( + self, group_id: PlanningGroupID, joint_state: JointState | None = None + ) -> PoseStamped: + """Get planning group target-frame pose using current state by default.""" + robot_id = self._robot_id_for_group(group_id) with self._world.scratch_context() as ctx: - # If no state provided, fetch current from state monitor if joint_state is None: joint_state = self.get_current_joint_state(robot_id) - if joint_state is not None: self._world.set_joint_state(ctx, robot_id, joint_state) - return self._world.get_ee_pose(ctx, robot_id) + return self._world.get_group_pose(ctx, group_id) def get_link_pose( self, robot_id: WorldRobotID, link_name: str, joint_state: JointState | None = None @@ -387,8 +404,6 @@ def get_link_pose( link_name: Name of the link in the URDF joint_state: Joint state to use (uses current if None) """ - from dimos.msgs.geometry_msgs.Quaternion import Quaternion - with self._world.scratch_context() as ctx: if joint_state is None: joint_state = self.get_current_joint_state(robot_id) @@ -410,10 +425,44 @@ def get_link_pose( ) def get_jacobian(self, robot_id: WorldRobotID, joint_state: JointState) -> NDArray[np.float64]: - """Get 6xN Jacobian matrix.""" + """Get 6xN Jacobian for the robot's unique pose-targetable group.""" + return self.get_group_jacobian( + self._unique_pose_group_id_for_robot(robot_id), + joint_state=joint_state, + ) + + def get_group_jacobian( + self, group_id: PlanningGroupID, joint_state: JointState + ) -> NDArray[np.float64]: + """Get planning group target-frame 6xN Jacobian matrix.""" + self._planning_groups.get(group_id) + robot_id = self._robot_id_for_group(group_id) with self._world.scratch_context() as ctx: self._world.set_joint_state(ctx, robot_id, joint_state) - return self._world.get_jacobian(ctx, robot_id) + return self._world.get_group_jacobian(ctx, group_id) + + def _unique_pose_group_id_for_robot(self, robot_id: WorldRobotID) -> PlanningGroupID: + robot_name = self._world.get_robot_config(robot_id).name + pose_group_ids = [ + group.id + for group in self._planning_groups.groups_for_robot(robot_name) + if group.has_pose_target + ] + if len(pose_group_ids) != 1: + raise ValueError( + f"Robot '{robot_name}' has {len(pose_group_ids)} pose-targetable planning groups; " + "call get_group_pose/get_group_jacobian with an explicit planning group ID" + ) + return pose_group_ids[0] + + def _robot_id_for_group(self, group_id: PlanningGroupID) -> WorldRobotID: + group = self._planning_groups.get(group_id) + try: + return self._robot_ids_by_name[group.robot_name] + except KeyError as exc: + raise KeyError( + f"Robot '{group.robot_name}' not found for planning group {group_id}" + ) from exc # Lifecycle @@ -442,25 +491,20 @@ def publish_visualization(self) -> None: if self._visualization is not None: self._visualization.publish_visualization() - def show_preview(self, robot_id: WorldRobotID) -> None: - """Show the preview representation for a robot if visualization is available.""" + def show_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Show preview representation for planning groups if visualization is available.""" if self._visualization is not None: - self._visualization.show_preview(robot_id) + self._visualization.show_preview(group_ids) - def hide_preview(self, robot_id: WorldRobotID) -> None: - """Hide the preview representation for a robot if visualization is available.""" + def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Hide preview representation for planning groups if visualization is available.""" if self._visualization is not None: - self._visualization.hide_preview(robot_id) + self._visualization.hide_preview(group_ids) - def animate_path( - self, - robot_id: WorldRobotID, - path: JointPath, - duration: float = 3.0, - ) -> None: - """Animate a path if visualization is available.""" + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: + """Animate a generated plan if visualization is available.""" if self._visualization is not None: - self._visualization.animate_path(robot_id, path, duration) + self._visualization.animate_plan(plan, duration) def start_visualization_thread(self, rate_hz: float = 10.0) -> None: """Start background thread for visualization updates at given rate.""" diff --git a/dimos/manipulation/planning/planners/rrt_planner.py b/dimos/manipulation/planning/planners/rrt_planner.py index 3ca19eb099..1e2d4fc396 100644 --- a/dimos/manipulation/planning/planners/rrt_planner.py +++ b/dimos/manipulation/planning/planners/rrt_planner.py @@ -26,8 +26,18 @@ import numpy as np +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection +from dimos.manipulation.planning.planning_identifiers import ( + local_joint_name_from_global, + make_global_joint_names, +) from dimos.manipulation.planning.spec.enums import PlanningStatus -from dimos.manipulation.planning.spec.models import JointPath, PlanningResult, WorldRobotID +from dimos.manipulation.planning.spec.models import ( + JointPath, + PlanningResult, + RobotName, + WorldRobotID, +) from dimos.manipulation.planning.spec.protocols import WorldSpec from dimos.manipulation.planning.utils.path_utils import compute_path_length from dimos.msgs.sensor_msgs.JointState import JointState @@ -98,6 +108,13 @@ def plan_joint_path( if error is not None: return error + if world.check_edge_collision_free(robot_id, start, goal, self._collision_step_size): + return _create_success_result( + [start, goal], + time.time() - start_time, + 0, + ) + lower, upper = world.get_joint_limits(robot_id) start_tree = [TreeNode(config=q_start.copy())] goal_tree = [TreeNode(config=q_goal.copy())] @@ -147,6 +164,286 @@ def get_name(self) -> str: """Get planner name.""" return "RRTConnect" + def plan_selected_joint_path( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + start: JointState, + goal: JointState, + timeout: float = 10.0, + ) -> PlanningResult: + """Plan a collision-free path for an explicit planning-group selection.""" + selected_joint_names = [ + joint_name for group in selection.groups for joint_name in group.joint_names + ] + exact_error = _validate_exact_joint_keys(start, selected_joint_names, "start") + if exact_error is not None: + return exact_error + exact_error = _validate_exact_joint_keys(goal, selected_joint_names, "goal") + if exact_error is not None: + return exact_error + + try: + robot_ids_by_name = _robot_ids_by_name(world, selection.robot_names) + except (KeyError, ValueError) as exc: + return _create_failure_result(PlanningStatus.INVALID_GOAL, str(exc)) + + robot_ids = set(robot_ids_by_name.values()) + if len(robot_ids) != 1: + return self._plan_multi_robot_selected_joint_path( + world=world, + groups=selection.groups, + robot_ids_by_name=robot_ids_by_name, + start=start, + goal=goal, + timeout=timeout, + ) + + robot_id = next(iter(robot_ids)) + robot_config = world.get_robot_config(robot_id) + full_global_joint_names = make_global_joint_names( + robot_config.name, robot_config.joint_names + ) + if selected_joint_names != full_global_joint_names: + return _create_failure_result( + PlanningStatus.UNSUPPORTED, + "RRTConnectPlanner currently requires the selected groups to cover " + "the robot controllable joint set exactly", + ) + + local_start = _global_joint_state_to_local( + start, + robot_config.name, + list(robot_config.joint_names), + selected_joint_names, + ) + local_goal = _global_joint_state_to_local( + goal, + robot_config.name, + list(robot_config.joint_names), + selected_joint_names, + ) + result = self.plan_joint_path( + world=world, + robot_id=robot_id, + start=local_start, + goal=local_goal, + timeout=timeout, + ) + if not result.is_success(): + return result + return PlanningResult( + status=result.status, + path=_local_path_to_global(result.path, robot_config.name, selected_joint_names), + planning_time=result.planning_time, + path_length=result.path_length, + iterations=result.iterations, + message=result.message, + timestamps=result.timestamps, + ) + + def _plan_multi_robot_selected_joint_path( + self, + world: WorldSpec, + groups: tuple[PlanningGroup, ...], + robot_ids_by_name: dict[RobotName, WorldRobotID], + start: JointState, + goal: JointState, + timeout: float, + ) -> PlanningResult: + """Plan over one coupled configuration vector for all selected robots.""" + start_time = time.time() + + if not world.is_finalized: + return _create_failure_result( + PlanningStatus.NO_SOLUTION, + "World must be finalized before planning", + ) + + selected_joint_names = [joint for group in groups for joint in group.joint_names] + q_start = np.array( + _order_joint_state(start, selected_joint_names).position, dtype=np.float64 + ) + q_goal = np.array(_order_joint_state(goal, selected_joint_names).position, dtype=np.float64) + + try: + robot_order, robot_joint_names = _validate_full_robot_groups( + world, groups, robot_ids_by_name + ) + except KeyError as exc: + return _create_failure_result(PlanningStatus.NO_SOLUTION, str(exc)) + if not robot_order: + return _create_failure_result( + PlanningStatus.INVALID_GOAL, "No planning groups selected" + ) + + unsupported = _validate_selected_groups_cover_full_robots( + world, robot_order, robot_joint_names + ) + if unsupported is not None: + return unsupported + + lower, upper = _combined_joint_limits(world, robot_order) + + if not _coupled_config_collision_free( + world, robot_order, robot_joint_names, selected_joint_names, q_start + ): + return _create_failure_result( + PlanningStatus.COLLISION_AT_START, + "Start configuration is in collision", + ) + if not _coupled_config_collision_free( + world, robot_order, robot_joint_names, selected_joint_names, q_goal + ): + return _create_failure_result( + PlanningStatus.COLLISION_AT_GOAL, + "Goal configuration is in collision", + ) + + if np.any(q_start < lower) or np.any(q_start > upper): + return _create_failure_result( + PlanningStatus.INVALID_START, + "Start configuration is outside joint limits", + ) + if np.any(q_goal < lower) or np.any(q_goal > upper): + return _create_failure_result( + PlanningStatus.INVALID_GOAL, + "Goal configuration is outside joint limits", + ) + + if _coupled_edge_collision_free( + world, + robot_order, + robot_joint_names, + selected_joint_names, + q_start, + q_goal, + self._collision_step_size, + ): + return _create_success_result( + [start, goal], + time.time() - start_time, + 0, + ) + + start_tree = [TreeNode(config=q_start.copy())] + goal_tree = [TreeNode(config=q_goal.copy())] + trees_swapped = False + + max_iterations = 5000 + for iteration in range(max_iterations): + if time.time() - start_time > timeout: + return _create_failure_result( + PlanningStatus.TIMEOUT, + f"Timeout after {iteration} iterations", + time.time() - start_time, + iteration, + ) + + sample = np.random.uniform(lower, upper) + extended = self._extend_coupled_tree( + world, + robot_order, + robot_joint_names, + start_tree, + sample, + self._step_size, + selected_joint_names, + ) + + if extended is not None: + connected = self._connect_coupled_tree( + world, + robot_order, + robot_joint_names, + goal_tree, + extended.config, + self._connect_step_size, + selected_joint_names, + ) + if connected is not None: + path = self._extract_path(extended, connected, selected_joint_names) + if trees_swapped: + path = list(reversed(path)) + path = _simplify_coupled_path( + world, + robot_order, + robot_joint_names, + path, + self._collision_step_size, + ) + return _create_success_result(path, time.time() - start_time, iteration + 1) + + start_tree, goal_tree = goal_tree, start_tree + trees_swapped = not trees_swapped + + return _create_failure_result( + PlanningStatus.NO_SOLUTION, + f"No path found after {max_iterations} iterations", + time.time() - start_time, + max_iterations, + ) + + def _extend_coupled_tree( + self, + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], + tree: list[TreeNode], + target: NDArray[np.float64], + step_size: float, + selected_joint_names: list[str], + ) -> TreeNode | None: + """Extend a tree in the coupled selected-joint configuration space.""" + nearest = min(tree, key=lambda node: float(np.linalg.norm(node.config - target))) + diff = target - nearest.config + dist = float(np.linalg.norm(diff)) + if dist <= step_size: + new_config = target.copy() + else: + new_config = nearest.config + step_size * (diff / dist) + + if _coupled_edge_collision_free( + world, + robot_order, + robot_joint_names, + selected_joint_names, + nearest.config, + new_config, + self._collision_step_size, + ): + new_node = TreeNode(config=new_config, parent=nearest) + nearest.children.append(new_node) + tree.append(new_node) + return new_node + return None + + def _connect_coupled_tree( + self, + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], + tree: list[TreeNode], + target: NDArray[np.float64], + step_size: float, + selected_joint_names: list[str], + ) -> TreeNode | None: + """Try to connect a coupled tree to a target configuration.""" + while True: + result = self._extend_coupled_tree( + world, + robot_order, + robot_joint_names, + tree, + target, + step_size, + selected_joint_names, + ) + if result is None: + return None + if float(np.linalg.norm(result.config - target)) < self._goal_tolerance: + return result + def _validate_inputs( self, world: WorldSpec, @@ -344,3 +641,251 @@ def _create_failure_result( iterations=iterations, message=message, ) + + +def _validate_full_robot_groups( + world: WorldSpec, + groups: tuple[PlanningGroup, ...], + robot_ids_by_name: dict[RobotName, WorldRobotID], +) -> tuple[list[WorldRobotID], dict[WorldRobotID, list[str]]]: + robot_order: list[WorldRobotID] = [] + robot_joint_names: dict[WorldRobotID, list[str]] = {} + known_robot_ids = set(world.get_robot_ids()) + for group in groups: + robot_id = robot_ids_by_name[group.robot_name] + if robot_id not in known_robot_ids: + raise KeyError(f"Robot '{robot_id}' not found") + if robot_id not in robot_joint_names: + robot_joint_names[robot_id] = [] + robot_order.append(robot_id) + robot_joint_names[robot_id].extend(group.joint_names) + return robot_order, robot_joint_names + + +def _robot_ids_by_name( + world: WorldSpec, robot_names: tuple[RobotName, ...] +) -> dict[RobotName, WorldRobotID]: + robot_ids_by_name: dict[RobotName, WorldRobotID] = {} + for robot_name in robot_names: + matches = [ + robot_id + for robot_id in world.get_robot_ids() + if world.get_robot_config(robot_id).name == robot_name + ] + if not matches: + raise KeyError(f"Robot '{robot_name}' not found") + if len(matches) > 1: + raise ValueError(f"Robot name '{robot_name}' is not unique in planning world") + robot_ids_by_name[robot_name] = matches[0] + return robot_ids_by_name + + +def _validate_selected_groups_cover_full_robots( + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], +) -> PlanningResult | None: + for robot_id in robot_order: + robot_config = world.get_robot_config(robot_id) + full_global_joint_names = make_global_joint_names( + robot_config.name, robot_config.joint_names + ) + if robot_joint_names[robot_id] != full_global_joint_names: + return _create_failure_result( + PlanningStatus.UNSUPPORTED, + "RRTConnectPlanner currently requires selected groups to cover " + "each affected robot's controllable joint set exactly", + ) + return None + + +def _combined_joint_limits( + world: WorldSpec, + robot_order: list[WorldRobotID], +) -> tuple[NDArray[np.float64], NDArray[np.float64]]: + lower_parts: list[NDArray[np.float64]] = [] + upper_parts: list[NDArray[np.float64]] = [] + for robot_id in robot_order: + lower, upper = world.get_joint_limits(robot_id) + lower_parts.append(lower) + upper_parts.append(upper) + return np.concatenate(lower_parts), np.concatenate(upper_parts) + + +def _robot_joint_state_from_combined( + combined_joint_names: list[str], + combined_positions: NDArray[np.float64], + robot_name: str, + robot_joint_names: list[str], +) -> JointState: + position_by_name = dict(zip(combined_joint_names, combined_positions.tolist(), strict=True)) + return JointState( + name=[local_joint_name_from_global(robot_name, name) for name in robot_joint_names], + position=[position_by_name[name] for name in robot_joint_names], + ) + + +def _global_joint_state_to_local( + joint_state: JointState, + robot_name: str, + robot_joint_names: list[str], + global_joint_names: list[str], +) -> JointState: + position_by_name = dict(zip(joint_state.name, joint_state.position, strict=True)) + local_joint_names = [ + local_joint_name_from_global(robot_name, name) for name in global_joint_names + ] + if local_joint_names != robot_joint_names: + raise ValueError("Global selected joints do not match robot joint order") + return JointState( + name=robot_joint_names, + position=[position_by_name[global_name] for global_name in global_joint_names], + ) + + +def _local_path_to_global( + path: JointPath, + robot_name: str, + global_joint_names: list[str], +) -> JointPath: + local_joint_names = [ + local_joint_name_from_global(robot_name, name) for name in global_joint_names + ] + global_path: JointPath = [] + for waypoint in path: + position_by_name = dict(zip(waypoint.name, waypoint.position, strict=True)) + global_path.append( + JointState( + name=global_joint_names, + position=[position_by_name[local_name] for local_name in local_joint_names], + ) + ) + return global_path + + +def _coupled_config_collision_free( + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], + selected_joint_names: list[str], + q: NDArray[np.float64], +) -> bool: + with world.scratch_context() as ctx: + for robot_id in robot_order: + world.set_joint_state( + ctx, + robot_id, + _robot_joint_state_from_combined( + selected_joint_names, + q, + world.get_robot_config(robot_id).name, + robot_joint_names[robot_id], + ), + ) + return all(world.is_collision_free(ctx, robot_id) for robot_id in robot_order) + + +def _coupled_edge_collision_free( + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], + selected_joint_names: list[str], + q_start: NDArray[np.float64], + q_end: NDArray[np.float64], + step_size: float, +) -> bool: + dist = float(np.linalg.norm(q_end - q_start)) + if dist < 1e-8: + return _coupled_config_collision_free( + world, + robot_order, + robot_joint_names, + selected_joint_names, + q_start, + ) + + n_steps = max(2, int(np.ceil(dist / step_size)) + 1) + with world.scratch_context() as ctx: + for i in range(n_steps): + t = i / (n_steps - 1) + q = q_start + t * (q_end - q_start) + for robot_id in robot_order: + world.set_joint_state( + ctx, + robot_id, + _robot_joint_state_from_combined( + selected_joint_names, + q, + world.get_robot_config(robot_id).name, + robot_joint_names[robot_id], + ), + ) + if not all(world.is_collision_free(ctx, robot_id) for robot_id in robot_order): + return False + return True + + +def _simplify_coupled_path( + world: WorldSpec, + robot_order: list[WorldRobotID], + robot_joint_names: dict[WorldRobotID, list[str]], + path: JointPath, + collision_step_size: float, + max_iterations: int = 100, +) -> JointPath: + if len(path) <= 2: + return path + + simplified = list(path) + selected_joint_names = list(path[0].name) + for _ in range(max_iterations): + if len(simplified) <= 2: + break + i = np.random.randint(0, len(simplified) - 2) + j = np.random.randint(i + 2, len(simplified)) + q_start = np.array(simplified[i].position, dtype=np.float64) + q_end = np.array(simplified[j].position, dtype=np.float64) + if _coupled_edge_collision_free( + world, + robot_order, + robot_joint_names, + selected_joint_names, + q_start, + q_end, + collision_step_size, + ): + simplified = simplified[: i + 1] + simplified[j:] + return simplified + + +def _validate_exact_joint_keys( + joint_state: JointState, selected_joint_names: list[str], state_name: str +) -> PlanningResult | None: + actual_names = list(joint_state.name) + expected_names = selected_joint_names + if set(actual_names) != set(expected_names): + missing = [name for name in expected_names if name not in actual_names] + extra = [name for name in actual_names if name not in expected_names] + details: list[str] = [] + if missing: + details.append(f"missing={missing}") + if extra: + details.append(f"extra={extra}") + return _create_failure_result( + PlanningStatus.INVALID_START if state_name == "start" else PlanningStatus.INVALID_GOAL, + f"{state_name} joint names must exactly match selected joints ({', '.join(details)})", + ) + if len(joint_state.position) != len(joint_state.name): + return _create_failure_result( + PlanningStatus.INVALID_START if state_name == "start" else PlanningStatus.INVALID_GOAL, + f"{state_name} joint name and position lengths must match", + ) + return None + + +def _order_joint_state(joint_state: JointState, joint_names: list[str]) -> JointState: + position_by_name = dict(zip(joint_state.name, joint_state.position, strict=False)) + return JointState( + name=joint_names, + position=[position_by_name[name] for name in joint_names], + ) diff --git a/dimos/manipulation/planning/planners/test_rrt_planner_selection.py b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py new file mode 100644 index 0000000000..4436aba595 --- /dev/null +++ b/dimos/manipulation/planning/planners/test_rrt_planner_selection.py @@ -0,0 +1,247 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for selected-joint RRT planning group contracts.""" + +from __future__ import annotations + +from collections.abc import Callable +from contextlib import nullcontext +from pathlib import Path +from typing import cast + +import numpy as np + +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection +from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.protocols import WorldSpec +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState + + +def _pose() -> PoseStamped: + return PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]) + + +def _robot_config(name: str, joint_names: list[str]) -> RobotModelConfig: + return RobotModelConfig( + name=name, + model_path=Path("robot.urdf"), + base_pose=_pose(), + joint_names=joint_names, + end_effector_link="tool0", + ) + + +def _joint_state(names: list[str], positions: list[float]) -> JointState: + return JointState({"name": names, "position": positions}) + + +def _group( + group_id: str, + robot_name: str, + joint_names: tuple[str, ...], +) -> PlanningGroup: + return PlanningGroup( + id=group_id, + robot_name=robot_name, + group_name=group_id.split("/", maxsplit=1)[1], + joint_names=joint_names, + local_joint_names=tuple(name.split("/", maxsplit=1)[1] for name in joint_names), + base_link="base_link", + tip_link="tool0", + ) + + +def _selection(*groups: PlanningGroup) -> PlanningGroupSelection: + return PlanningGroupSelection.from_groups(tuple(groups)) + + +class _SelectionWorld: + is_finalized = True + + def __init__( + self, + robot_configs: dict[str, RobotModelConfig], + coupled_collision_predicate: Callable[[dict[str, JointState]], bool] | None = None, + ) -> None: + self._robot_configs = robot_configs + self._coupled_collision_predicate = coupled_collision_predicate + self.coupled_collision_checks = 0 + self.config_collision_names: list[list[str]] = [] + self.edge_collision_names: list[tuple[list[str], list[str]]] = [] + + def get_robot_config(self, robot_id: str) -> RobotModelConfig: + return self._robot_configs[robot_id] + + def get_robot_ids(self) -> list[str]: + return list(self._robot_configs) + + def check_config_collision_free(self, robot_id: str, joint_state: JointState) -> bool: + self.config_collision_names.append(list(joint_state.name)) + return True + + def get_joint_limits(self, robot_id: str) -> tuple[np.ndarray, np.ndarray]: + joint_count = len(self._robot_configs[robot_id].joint_names) + return -np.ones(joint_count), np.ones(joint_count) + + def check_edge_collision_free( + self, + robot_id: str, + start: JointState, + goal: JointState, + step_size: float, + ) -> bool: + self.edge_collision_names.append((list(start.name), list(goal.name))) + return True + + def scratch_context(self) -> nullcontext[dict[str, JointState]]: + return nullcontext({}) + + def set_joint_state( + self, ctx: dict[str, JointState], robot_id: str, joint_state: JointState + ) -> None: + assert joint_state.name == self._robot_configs[robot_id].joint_names + ctx[robot_id] = joint_state + + def is_collision_free(self, ctx: dict[str, JointState], robot_id: str) -> bool: + self.coupled_collision_checks += 1 + if self._coupled_collision_predicate is None: + return True + return self._coupled_collision_predicate(ctx) + + +def test_plan_selected_joint_path_rejects_missing_and_extra_start_names() -> None: + group = _group("arm/arm", "arm", ("arm/joint1", "arm/joint2")) + world = _SelectionWorld(robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + _selection(group), + start=_joint_state(["arm/joint1", "arm/extra"], [0.0, 0.0]), + goal=_joint_state(["arm/joint1", "arm/joint2"], [0.0, 0.0]), + ) + + assert result.status == PlanningStatus.INVALID_START + assert "missing" in result.message + assert "extra" in result.message + + +def test_plan_selected_joint_path_rejects_missing_and_extra_goal_names() -> None: + group = _group("arm/arm", "arm", ("arm/joint1", "arm/joint2")) + world = _SelectionWorld(robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + _selection(group), + start=_joint_state(["arm/joint1", "arm/joint2"], [0.0, 0.0]), + goal=_joint_state(["arm/joint1", "arm/extra"], [0.0, 0.0]), + ) + + assert result.status == PlanningStatus.INVALID_GOAL + assert "missing" in result.message + assert "extra" in result.message + + +def test_plan_selected_joint_path_plans_cross_robot_full_group_selection() -> None: + left_group = _group("left/arm", "left", ("left/joint1",)) + right_group = _group("right/arm", "right", ("right/joint1",)) + world = _SelectionWorld( + robot_configs={ + "left_robot": _robot_config("left", ["joint1"]), + "right_robot": _robot_config("right", ["joint1"]), + }, + ) + joint_state = _joint_state(["left/joint1", "right/joint1"], [0.0, 0.0]) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + _selection(left_group, right_group), + start=joint_state, + goal=_joint_state(["left/joint1", "right/joint1"], [0.1, -0.1]), + ) + + assert result.status == PlanningStatus.SUCCESS + assert len(result.path) == 2 + assert result.path[0].name == ["left/joint1", "right/joint1"] + assert result.path[-1].position == [0.1, -0.1] + assert world.coupled_collision_checks > 0 + + +def test_plan_selected_joint_path_converts_single_robot_backend_boundary_to_local() -> None: + group = _group("arm/manipulator", "arm", ("arm/joint1", "arm/joint2")) + world = _SelectionWorld(robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + _selection(group), + start=_joint_state(["arm/joint2", "arm/joint1"], [0.2, 0.1]), + goal=_joint_state(["arm/joint1", "arm/joint2"], [0.3, 0.4]), + ) + + assert result.status == PlanningStatus.SUCCESS + assert [waypoint.name for waypoint in result.path] == [ + ["arm/joint1", "arm/joint2"], + ["arm/joint1", "arm/joint2"], + ] + assert result.path[0].position == [0.1, 0.2] + assert result.path[-1].position == [0.3, 0.4] + assert world.config_collision_names == [["joint1", "joint2"], ["joint1", "joint2"]] + assert world.edge_collision_names == [(["joint1", "joint2"], ["joint1", "joint2"])] + + +def test_plan_selected_joint_path_rejects_cross_robot_coupled_goal_collision() -> None: + def coupled_free(ctx: dict[str, JointState]) -> bool: + if {"left_robot", "right_robot"} - set(ctx): + return True + left = ctx["left_robot"].position[0] + right = ctx["right_robot"].position[0] + return not (left > 0.04 and right > 0.04) + + left_group = _group("left/arm", "left", ("left/joint1",)) + right_group = _group("right/arm", "right", ("right/joint1",)) + world = _SelectionWorld( + robot_configs={ + "left_robot": _robot_config("left", ["joint1"]), + "right_robot": _robot_config("right", ["joint1"]), + }, + coupled_collision_predicate=coupled_free, + ) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + _selection(left_group, right_group), + start=_joint_state(["left/joint1", "right/joint1"], [0.0, 0.0]), + goal=_joint_state(["left/joint1", "right/joint1"], [0.1, 0.1]), + ) + + assert result.status == PlanningStatus.COLLISION_AT_GOAL + assert world.coupled_collision_checks > 0 + + +def test_plan_selected_joint_path_rejects_single_robot_subset_selection() -> None: + group = _group("arm/wrist", "arm", ("arm/joint2",)) + world = _SelectionWorld(robot_configs={"robot_1": _robot_config("arm", ["joint1", "joint2"])}) + joint_state = _joint_state(["arm/joint2"], [0.0]) + + result = RRTConnectPlanner().plan_selected_joint_path( + cast("WorldSpec", world), + _selection(group), + start=joint_state, + goal=joint_state, + ) + + assert result.status == PlanningStatus.UNSUPPORTED diff --git a/dimos/manipulation/planning/planning_identifiers.py b/dimos/manipulation/planning/planning_identifiers.py new file mode 100644 index 0000000000..2f7c479e5e --- /dev/null +++ b/dimos/manipulation/planning/planning_identifiers.py @@ -0,0 +1,126 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Planning and joint identifier helpers.""" + +from __future__ import annotations + +from collections.abc import Sequence + +from dimos.manipulation.planning.spec.models import ( + GlobalJointName, + LocalModelJointName, + PlanningGroupID, + RobotName, +) + + +def assert_valid_robot_name(robot_name: RobotName) -> None: + """Validate a robot name for delimiter-based public IDs.""" + if not robot_name or "/" in robot_name: + raise ValueError(f"Invalid robot name: {robot_name!r}") + + +def assert_valid_local_joint_name(local_joint_name: LocalModelJointName) -> None: + """Validate a local model joint name for delimiter-based global joint names.""" + if not local_joint_name or "/" in local_joint_name: + raise ValueError(f"Invalid local joint name: {local_joint_name!r}") + + +def assert_local_joint_names(names: Sequence[LocalModelJointName]) -> None: + """Validate that names are local model joint names, not global joint names.""" + for name in names: + assert_valid_local_joint_name(name) + + +def make_planning_group_id(robot_name: RobotName, group_name: str) -> PlanningGroupID: + """Build a public planning group ID.""" + assert_valid_robot_name(robot_name) + if not group_name or "/" in group_name: + raise ValueError(f"Invalid planning group name: {group_name!r}") + return f"{robot_name}/{group_name}" + + +def parse_planning_group_id(group_id: PlanningGroupID) -> tuple[RobotName, str]: + """Split and validate a planning group ID.""" + parts = group_id.split("/", maxsplit=1) + if len(parts) != 2 or not parts[0] or not parts[1] or "/" in parts[1]: + raise ValueError( + f"Invalid planning group ID {group_id!r}; expected '{{robot_name}}/{{group_name}}'" + ) + return parts[0], parts[1] + + +def make_global_joint_name( + robot_name: RobotName, + local_joint_name: LocalModelJointName, +) -> GlobalJointName: + """Convert a local model joint name to a public global joint name.""" + assert_valid_robot_name(robot_name) + assert_valid_local_joint_name(local_joint_name) + return f"{robot_name}/{local_joint_name}" + + +def make_global_joint_names( + robot_name: RobotName, + local_joint_names: list[LocalModelJointName] | tuple[LocalModelJointName, ...], +) -> list[GlobalJointName]: + """Convert local model joint names to public global joint names.""" + return [make_global_joint_name(robot_name, name) for name in local_joint_names] + + +def is_global_joint_name(name: str) -> bool: + """Return whether name has the exact global joint-name shape.""" + parts = name.split("/") + return len(parts) == 2 and bool(parts[0]) and bool(parts[1]) + + +def assert_global_joint_names(names: Sequence[GlobalJointName]) -> None: + """Validate that names are global joint names.""" + invalid = [name for name in names if not is_global_joint_name(name)] + if invalid: + raise ValueError(f"Expected global joint names; got invalid names: {invalid}") + + +def local_joint_name_from_global( + robot_name: RobotName, + global_joint_name: GlobalJointName, +) -> LocalModelJointName: + """Validate and strip a global joint name for backend internals.""" + assert_valid_robot_name(robot_name) + prefix = f"{robot_name}/" + if not global_joint_name.startswith(prefix): + raise ValueError( + f"Global joint name {global_joint_name!r} does not belong to robot {robot_name!r}" + ) + local_name = global_joint_name[len(prefix) :] + try: + assert_valid_local_joint_name(local_name) + except ValueError as exc: + raise ValueError(f"Invalid global joint name: {global_joint_name!r}") from exc + return local_name + + +__all__ = [ + "assert_global_joint_names", + "assert_local_joint_names", + "assert_valid_local_joint_name", + "assert_valid_robot_name", + "is_global_joint_name", + "local_joint_name_from_global", + "make_global_joint_name", + "make_global_joint_names", + "make_planning_group_id", + "parse_planning_group_id", +] diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index 74dc3bd69b..cb795b2fbb 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -21,6 +21,12 @@ from pydantic import Field from dimos.core.module import ModuleConfig +from dimos.manipulation.planning.groups.discovery import FALLBACK_PLANNING_GROUP_NAME +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition +from dimos.manipulation.planning.planning_identifiers import ( + assert_local_joint_names, + assert_valid_robot_name, +) from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -30,10 +36,19 @@ class RobotModelConfig(ModuleConfig): Attributes: name: Human-readable robot name model_path: Path to robot model file (.urdf, .xacro, or .xml/MJCF) - base_pose: Pose of robot base in world frame (position + orientation) - joint_names: Ordered list of controlled joint names (in URDF namespace) - end_effector_link: Name of the end-effector link for FK/IK - base_link: Name of the base link (default: "base_link") + srdf_path: Optional path to SRDF file containing planning group definitions + base_pose: Compatibility placement transform used by current Drake + world loading/welding. This is the canonical world placement for + robot instances; model-authored world/base attach joints are + stripped when strip_model_world_joint is true. + joint_names: Ordered list of controllable joints in the local model + namespace. This is not a planning group. + end_effector_link: Compatibility robot-scoped end-effector link used by + legacy helpers. New pose-targeted planning should use planning + group target frames instead. + base_link: Compatibility robot-scoped base link used by current Drake + weld/placement behavior. Planning groups own chain base links. + TODO: should remove package_paths: Dict mapping package names to filesystem Paths joint_limits_lower: Lower joint limits (radians) joint_limits_upper: Upper joint limits (radians) @@ -45,19 +60,19 @@ class RobotModelConfig(ModuleConfig): links may legitimately overlap (e.g., mimic joints). max_velocity: Maximum joint velocity for trajectory generation (rad/s) max_acceleration: Maximum joint acceleration for trajectory generation (rad/s^2) - joint_name_mapping: Maps coordinator joint names to URDF joint names. - Example: {"left/joint1": "joint1"} means coordinator's "left/joint1" - corresponds to URDF's "joint1". If empty, names are assumed to match. coordinator_task_name: Task name for executing trajectories via coordinator RPC. If set, trajectories can be executed via execute_trajectory() RPC. """ name: str model_path: Path - base_pose: PoseStamped + srdf_path: Path | None = None + base_pose: PoseStamped = Field(default_factory=PoseStamped) + strip_model_world_joint: bool = False joint_names: list[str] - end_effector_link: str + end_effector_link: str | None = None base_link: str = "base_link" + planning_groups: list[PlanningGroupDefinition] = Field(default_factory=list) package_paths: dict[str, Path] = Field(default_factory=dict) joint_limits_lower: list[float] | None = None joint_limits_upper: list[float] | None = None @@ -69,7 +84,6 @@ class RobotModelConfig(ModuleConfig): max_velocity: float = 1.0 max_acceleration: float = 2.0 # Coordinator integration - joint_name_mapping: dict[str, str] = Field(default_factory=dict) coordinator_task_name: str | None = None gripper_hardware_id: str | None = None # TF publishing for extra links (e.g., camera mount) @@ -79,19 +93,17 @@ class RobotModelConfig(ModuleConfig): # Pre-grasp offset distance in meters (along approach direction) pre_grasp_offset: float = 0.10 - def get_urdf_joint_name(self, coordinator_name: str) -> str: - """Translate coordinator joint name to URDF joint name.""" - return self.joint_name_mapping.get(coordinator_name, coordinator_name) - - def get_coordinator_joint_name(self, urdf_name: str) -> str: - """Translate URDF joint name to coordinator joint name.""" - for coord_name, u_name in self.joint_name_mapping.items(): - if u_name == urdf_name: - return coord_name - return urdf_name - - def get_coordinator_joint_names(self) -> list[str]: - """Get joint names in coordinator namespace.""" - if not self.joint_name_mapping: - return self.joint_names - return [self.get_coordinator_joint_name(j) for j in self.joint_names] + def model_post_init(self, __context: object) -> None: + """Validate delimiter-based naming constraints.""" + assert_valid_robot_name(self.name) + assert_local_joint_names(self.joint_names) + if not self.planning_groups: + self.planning_groups = [ + PlanningGroupDefinition( + name=FALLBACK_PLANNING_GROUP_NAME, + joint_names=tuple(self.joint_names), + base_link=self.base_link, + tip_link=self.end_effector_link, + source="fallback", + ) + ] diff --git a/dimos/manipulation/planning/spec/enums.py b/dimos/manipulation/planning/spec/enums.py index 66a17ee199..e1c7c1a735 100644 --- a/dimos/manipulation/planning/spec/enums.py +++ b/dimos/manipulation/planning/spec/enums.py @@ -47,3 +47,4 @@ class PlanningStatus(Enum): INVALID_GOAL = auto() COLLISION_AT_START = auto() COLLISION_AT_GOAL = auto() + UNSUPPORTED = auto() diff --git a/dimos/manipulation/planning/spec/models.py b/dimos/manipulation/planning/spec/models.py index d412e9f766..29e6209809 100644 --- a/dimos/manipulation/planning/spec/models.py +++ b/dimos/manipulation/planning/spec/models.py @@ -41,6 +41,15 @@ WorldRobotID: TypeAlias = str """Internal Drake world robot ID""" +PlanningGroupID: TypeAlias = str +"""Public planning group ID of the form {robot_name}/{group_name}.""" + +LocalModelJointName: TypeAlias = str +"""Joint name as it appears in URDF/SRDF before world binding.""" + +GlobalJointName: TypeAlias = str +"""Public joint name of the form {robot_name}/{local_joint_name}.""" + JointPath: TypeAlias = "list[JointState]" """List of joint states forming a path (each waypoint has names + positions)""" @@ -61,6 +70,27 @@ class PlanningSceneInfo: """6 x n Jacobian matrix (rows: [vx, vy, vz, wx, wy, wz])""" +@dataclass +class GeneratedPlan: + """Canonical generated planning artifact. + + The path uses global joint names and contains exactly the selected joints. + Downstream preview/execution projections are computed lazily from this data. + """ + + group_ids: tuple[PlanningGroupID, ...] + path: list[JointState] = field(default_factory=list) + status: PlanningStatus = PlanningStatus.NO_SOLUTION + planning_time: float = 0.0 + path_length: float = 0.0 + iterations: int = 0 + message: str = "" + + def is_success(self) -> bool: + """Check if planning was successful.""" + return self.status == PlanningStatus.SUCCESS + + @dataclass class Obstacle: """Obstacle specification for collision avoidance. diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index c7ee95ee0a..dfe7e9e427 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -23,16 +23,19 @@ from typing import TYPE_CHECKING, Any, Protocol, runtime_checkable if TYPE_CHECKING: + from collections.abc import Sequence from contextlib import AbstractContextManager import numpy as np from numpy.typing import NDArray + from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( + GeneratedPlan, IKResult, - JointPath, Obstacle, + PlanningGroupID, PlanningResult, PlanningSceneInfo, WorldRobotID, @@ -156,8 +159,19 @@ def check_edge_collision_free( ... # Forward Kinematics (require context) + def get_group_pose(self, ctx: Any, group_id: PlanningGroupID) -> PoseStamped: + """Get pose for a planning group's target frame.""" + ... + + def get_group_jacobian(self, ctx: Any, group_id: PlanningGroupID) -> NDArray[np.float64]: + """Get planning group target-frame Jacobian over the group's selected joints.""" + ... + def get_ee_pose(self, ctx: Any, robot_id: WorldRobotID) -> PoseStamped: - """Get end-effector pose.""" + """Get pose for a robot's unique pose-targetable planning group. + + TODO: deprecate this. + """ ... def get_link_pose( @@ -167,7 +181,7 @@ def get_link_pose( ... def get_jacobian(self, ctx: Any, robot_id: WorldRobotID) -> NDArray[np.float64]: - """Get end-effector Jacobian (6 x n_joints).""" + """Get Jacobian for a robot's unique pose-targetable planning group.""" ... @@ -194,16 +208,16 @@ def publish_visualization(self, ctx: Any | None = None) -> None: """Publish current state to visualization.""" ... - def show_preview(self, robot_id: WorldRobotID) -> None: - """Show the preview representation for a robot.""" + def show_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Show preview representations for the selected planning groups.""" ... - def hide_preview(self, robot_id: WorldRobotID) -> None: - """Hide the preview representation for a robot.""" + def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Hide preview representations for the selected planning groups.""" ... - def animate_path(self, robot_id: WorldRobotID, path: JointPath, duration: float = 3.0) -> None: - """Animate a path in visualization.""" + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: + """Animate a generated plan in visualization.""" ... def close(self) -> None: @@ -229,6 +243,20 @@ def solve( """Solve IK with optional collision checking.""" ... + def solve_pose_targets( + self, + world: WorldSpec, + pose_targets: dict[PlanningGroup, PoseStamped], + auxiliary_groups: list[PlanningGroup] | tuple[PlanningGroup, ...] = (), + seed: JointState | None = None, + position_tolerance: float = 0.001, + orientation_tolerance: float = 0.01, + check_collision: bool = True, + max_attempts: int = 10, + ) -> IKResult: + """Solve pose targets over planning groups plus request-scoped auxiliaries.""" + ... + @runtime_checkable class PlannerSpec(Protocol): @@ -254,6 +282,17 @@ def plan_joint_path( """Plan a collision-free joint-space path.""" ... + def plan_selected_joint_path( + self, + world: WorldSpec, + selection: PlanningGroupSelection, + start: JointState, + goal: JointState, + timeout: float = 10.0, + ) -> PlanningResult: + """Plan over an explicit planning-group selection.""" + ... + def get_name(self) -> str: """Get planner name.""" ... diff --git a/dimos/manipulation/planning/test_planning_group_utils.py b/dimos/manipulation/planning/test_planning_group_utils.py new file mode 100644 index 0000000000..d479f34a4e --- /dev/null +++ b/dimos/manipulation/planning/test_planning_group_utils.py @@ -0,0 +1,63 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Unit tests for planning group joint-name normalization.""" + +from __future__ import annotations + +import pytest + +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.groups.utils import joint_target_to_global_names +from dimos.msgs.sensor_msgs.JointState import JointState + + +def _make_group() -> PlanningGroup: + return PlanningGroup( + id="left/arm", + robot_name="left", + group_name="arm", + joint_names=("left/j1", "left/j2", "left/j3"), + local_joint_names=("j1", "j2", "j3"), + base_link="base", + tip_link="ee", + ) + + +def test_joint_target_to_global_names_accepts_named_global_targets_in_group_order() -> None: + group = _make_group() + target = JointState(name=["left/j3", "left/j1", "left/j2"], position=[3.0, 1.0, 2.0]) + + normalized = joint_target_to_global_names(group, target) + + assert normalized.name == ["left/j1", "left/j2", "left/j3"] + assert normalized.position == [1.0, 2.0, 3.0] + + +def test_joint_target_to_global_names_accepts_named_local_targets_in_group_order() -> None: + group = _make_group() + target = JointState(name=["j2", "j3", "j1"], position=[2.0, 3.0, 1.0]) + + normalized = joint_target_to_global_names(group, target) + + assert normalized.name == ["left/j1", "left/j2", "left/j3"] + assert normalized.position == [1.0, 2.0, 3.0] + + +def test_joint_target_to_global_names_rejects_mixed_global_and_local_target_names() -> None: + group = _make_group() + target = JointState(name=["left/j1", "j2", "left/j3"], position=[1.0, 2.0, 3.0]) + + with pytest.raises(ValueError, match="mixes global and local joint names"): + joint_target_to_global_names(group, target) diff --git a/dimos/manipulation/planning/test_planning_groups.py b/dimos/manipulation/planning/test_planning_groups.py new file mode 100644 index 0000000000..dd95d1b961 --- /dev/null +++ b/dimos/manipulation/planning/test_planning_groups.py @@ -0,0 +1,224 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for planning group discovery.""" + +from __future__ import annotations + +from pathlib import Path + +import pytest + +from dimos.manipulation.planning.groups.discovery import ( + FALLBACK_PLANNING_GROUP_NAME, + PlanningGroupDiscoveryError, + discover_planning_group_definitions, + generate_fallback_planning_group, + parse_srdf_planning_groups, +) +from dimos.robot.model_parser import JointDescription, ModelDescription + + +def _serial_model(*joint_types: str) -> ModelDescription: + joints = [ + JointDescription( + name=f"joint{i + 1}", + type=joint_type, + parent_link=f"link{i}", + child_link=f"link{i + 1}", + ) + for i, joint_type in enumerate(joint_types) + ] + return ModelDescription( + joints=joints, + root_link="link0", + links=[f"link{i}" for i in range(len(joint_types) + 1)], + ) + + +def _branching_model() -> ModelDescription: + return ModelDescription( + joints=[ + JointDescription( + name="left_joint", + type="revolute", + parent_link="base", + child_link="left_link", + ), + JointDescription( + name="right_joint", + type="revolute", + parent_link="base", + child_link="right_link", + ), + ], + root_link="base", + links=["base", "left_link", "right_link"], + ) + + +def _write_srdf(tmp_path: Path, body: str) -> Path: + srdf_path = tmp_path / "robot.srdf" + srdf_path.write_text(f"{body}") + return srdf_path + + +def test_parse_srdf_chain_group(tmp_path: Path) -> None: + model = _serial_model("revolute", "revolute", "revolute") + srdf_path = _write_srdf( + tmp_path, + "", + ) + + groups = parse_srdf_planning_groups( + srdf_path, + model=model, + controllable_joint_names=["joint1", "joint2", "joint3"], + ) + + assert len(groups) == 1 + assert groups[0].name == "arm" + assert groups[0].joint_names == ("joint1", "joint2", "joint3") + assert groups[0].base_link == "link0" + assert groups[0].tip_link == "link3" + assert groups[0].source == "srdf" + + +def test_parse_srdf_ordered_joint_list_group(tmp_path: Path) -> None: + model = _serial_model("revolute", "prismatic", "revolute") + srdf_path = _write_srdf( + tmp_path, + """ + + + + + + """, + ) + + groups = parse_srdf_planning_groups( + srdf_path, + model=model, + controllable_joint_names=["joint1", "joint2", "joint3"], + ) + + assert len(groups) == 1 + assert groups[0].joint_names == ("joint1", "joint2", "joint3") + assert groups[0].base_link == "link0" + assert groups[0].tip_link == "link3" + + +def test_parse_srdf_skips_unsupported_groups_and_ignores_end_effector( + tmp_path: Path, +) -> None: + model = _serial_model("revolute", "revolute") + srdf_path = _write_srdf( + tmp_path, + """ + + + + + """, + ) + + with pytest.warns(UserWarning) as warnings: + groups = parse_srdf_planning_groups( + srdf_path, + model=model, + controllable_joint_names=["joint1", "joint2"], + ) + + assert [group.name for group in groups] == ["arm"] + warning_text = "\n".join(str(warning.message) for warning in warnings) + assert "Skipping unsupported SRDF planning group links" in warning_text + assert "Skipping unsupported SRDF planning group nested" in warning_text + + +def test_fallback_generates_manipulator_for_unambiguous_serial_chain() -> None: + model = _serial_model("revolute", "prismatic", "revolute") + + group = generate_fallback_planning_group( + model=model, + controllable_joint_names=["joint2", "joint1", "joint3"], + ) + + assert group.name == FALLBACK_PLANNING_GROUP_NAME + assert group.joint_names == ("joint1", "joint2", "joint3") + assert group.base_link == "link0" + assert group.tip_link == "link3" + assert group.source == "fallback" + + +def test_fallback_strips_terminal_prismatic_joints() -> None: + model = _serial_model("revolute", "revolute", "prismatic") + + group = generate_fallback_planning_group( + model=model, + controllable_joint_names=["joint1", "joint2", "joint3"], + ) + + assert group.joint_names == ("joint1", "joint2") + assert group.tip_link == "link2" + + +def test_fallback_rejects_branching_model() -> None: + with pytest.raises(PlanningGroupDiscoveryError, match="branch"): + generate_fallback_planning_group( + model=_branching_model(), + controllable_joint_names=["left_joint", "right_joint"], + ) + + +def test_discovery_prefers_explicit_srdf_over_fallback(tmp_path: Path) -> None: + model = _serial_model("revolute", "revolute") + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + srdf_path = _write_srdf( + tmp_path, + "", + ) + + groups = discover_planning_group_definitions( + robot_name="robot", + model_path=model_path, + model=model, + controllable_joint_names=["joint1", "joint2"], + srdf_path=srdf_path, + ) + + assert [group.name for group in groups] == ["srdf_arm"] + + +def test_discovery_auto_discovers_srdf_with_warning( + tmp_path: Path, +) -> None: + model = _serial_model("revolute") + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + _write_srdf( + tmp_path, + "", + ) + + with pytest.warns(UserWarning, match="Auto-discovered SRDF"): + groups = discover_planning_group_definitions( + robot_name="robot", + model_path=model_path, + model=model, + controllable_joint_names=["joint1"], + ) + + assert [group.name for group in groups] == ["auto_arm"] diff --git a/dimos/manipulation/planning/utils/mesh_utils.py b/dimos/manipulation/planning/utils/mesh_utils.py index 988a4e5e8e..adb4e8f36f 100644 --- a/dimos/manipulation/planning/utils/mesh_utils.py +++ b/dimos/manipulation/planning/utils/mesh_utils.py @@ -37,6 +37,7 @@ import shutil import tempfile from typing import TYPE_CHECKING +import xml.etree.ElementTree as ET from dimos.utils.logging_config import setup_logger @@ -55,6 +56,7 @@ def prepare_urdf_for_drake( package_paths: dict[str, Path] | None = None, xacro_args: dict[str, str] | None = None, convert_meshes: bool = False, + strip_world_joint_child_link: str | None = None, ) -> str: """Prepare a URDF/xacro file for use with Drake. @@ -68,6 +70,9 @@ def prepare_urdf_for_drake( package_paths: Dict mapping package names to filesystem paths xacro_args: Arguments to pass to xacro processor convert_meshes: Convert DAE/STL meshes to OBJ for Drake compatibility + strip_world_joint_child_link: If set, remove a fixed URDF joint from + world to this child link so callers can apply instance placement via + RobotModelConfig.base_pose instead of model-authored placement. Returns: Path to the prepared URDF file (may be cached) @@ -77,7 +82,9 @@ def prepare_urdf_for_drake( xacro_args = xacro_args or {} # Generate cache key - cache_key = _generate_cache_key(urdf_path, package_paths, xacro_args, convert_meshes) + cache_key = _generate_cache_key( + urdf_path, package_paths, xacro_args, convert_meshes, strip_world_joint_child_link + ) cache_path = _CACHE_DIR / cache_key / urdf_path.stem cache_path.mkdir(parents=True, exist_ok=True) cached_urdf = cache_path / f"{urdf_path.stem}.urdf" @@ -96,6 +103,9 @@ def prepare_urdf_for_drake( # Strip transmission blocks (Drake doesn't need them, and they can cause issues) urdf_content = _strip_transmission_blocks(urdf_content) + if strip_world_joint_child_link is not None: + urdf_content = _strip_fixed_world_joint(urdf_content, strip_world_joint_child_link) + # Resolve package:// URIs urdf_content = _resolve_package_uris(urdf_content, package_paths, cache_path) @@ -115,6 +125,7 @@ def _generate_cache_key( package_paths: dict[str, Path], xacro_args: dict[str, str], convert_meshes: bool, + strip_world_joint_child_link: str | None, ) -> str: """Generate a cache key for the URDF configuration. @@ -125,9 +136,12 @@ def _generate_cache_key( # Version number to invalidate cache when processing logic changes # Increment this when adding new processing steps (e.g., stripping transmission blocks) - processing_version = "v2" + processing_version = "v3" - key_data = f"{processing_version}:{urdf_path}:{mtime}:{sorted(package_paths.items())}:{sorted(xacro_args.items())}:{convert_meshes}" + key_data = ( + f"{processing_version}:{urdf_path}:{mtime}:{sorted(package_paths.items())}:" + f"{sorted(xacro_args.items())}:{convert_meshes}:{strip_world_joint_child_link}" + ) return hashlib.md5(key_data.encode()).hexdigest()[:16] @@ -175,6 +189,44 @@ def _strip_transmission_blocks(urdf_content: str) -> str: return result +def _strip_fixed_world_joint(urdf_content: str, child_link: str) -> str: + """Remove a fixed world-to-base joint so base_pose can own placement.""" + try: + root = ET.fromstring(urdf_content) + except ET.ParseError: + logger.warning("Could not parse URDF while stripping world joint", exc_info=True) + return urdf_content + + removed = False + for joint in list(root.findall("joint")): + if joint.attrib.get("type") != "fixed": + continue + parent = joint.find("parent") + child = joint.find("child") + if parent is None or child is None: + continue + if parent.attrib.get("link") == "world" and child.attrib.get("link") == child_link: + root.remove(joint) + removed = True + + if not removed: + return urdf_content + + referenced_links = set() + for joint in root.findall("joint"): + parent = joint.find("parent") + child = joint.find("child") + if parent is not None: + referenced_links.add(parent.attrib.get("link")) + if child is not None: + referenced_links.add(child.attrib.get("link")) + for link in list(root.findall("link")): + if link.attrib.get("name") == "world" and "world" not in referenced_links: + root.remove(link) + + return ET.tostring(root, encoding="unicode") + + def _resolve_package_uris( urdf_content: str, package_paths: dict[str, Path], diff --git a/dimos/manipulation/planning/utils/test_mesh_utils.py b/dimos/manipulation/planning/utils/test_mesh_utils.py new file mode 100644 index 0000000000..fa3004c7ce --- /dev/null +++ b/dimos/manipulation/planning/utils/test_mesh_utils.py @@ -0,0 +1,47 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +import xml.etree.ElementTree as ET + +from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake + + +def test_prepare_urdf_can_strip_fixed_world_joint_for_base_pose_placement( + tmp_path, +) -> None: + urdf_path = tmp_path / "robot.urdf" + urdf_path.write_text( + """ + + + + + + + + + +""".strip() + ) + + prepared_path = prepare_urdf_for_drake( + urdf_path, + strip_world_joint_child_link="link_base", + ) + + root = ET.parse(prepared_path).getroot() + assert [joint.attrib["name"] for joint in root.findall("joint")] == [] + assert [link.attrib["name"] for link in root.findall("link")] == ["link_base"] diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index ca426ba340..23956c4a21 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -25,12 +25,17 @@ import numpy as np +from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry +from dimos.manipulation.planning.planning_identifiers import ( + assert_local_joint_names, + make_global_joint_name, +) from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import ObstacleType from dimos.manipulation.planning.spec.models import ( - JointPath, + GeneratedPlan, Obstacle, - PlanningSceneInfo, + PlanningGroupID, WorldRobotID, ) from dimos.manipulation.planning.spec.protocols import VisualizationSpec, WorldSpec @@ -38,7 +43,7 @@ from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: - from collections.abc import Generator + from collections.abc import Generator, Sequence from numpy.typing import NDArray @@ -84,6 +89,22 @@ logger = setup_logger() +def _pose_stamped_from_drake_transform(transform: RigidTransform) -> PoseStamped: + """Convert a Drake RigidTransform-like object to a world-frame pose.""" + position = transform.translation() + quaternion = transform.rotation().ToQuaternion() + return PoseStamped( + frame_id="world", + position=[float(position[0]), float(position[1]), float(position[2])], + orientation=[ + float(quaternion.x()), + float(quaternion.y()), + float(quaternion.z()), + float(quaternion.w()), + ], + ) + + @dataclass class _RobotData: """Internal data for tracking a robot in the world.""" @@ -92,8 +113,8 @@ class _RobotData: config: RobotModelConfig model_instance: Any # ModelInstanceIndex joint_indices: list[int] # Indices into plant's position vector - ee_frame: Any # BodyFrame for end-effector - base_frame: Any # BodyFrame for base + ee_frame: Any | None # Compatibility robot-scoped end-effector frame + base_frame: Any # Compatibility robot-scoped base frame preview_model_instance: Any = None # ModelInstanceIndex for preview (yellow) robot preview_joint_indices: list[int] = field(default_factory=list) @@ -184,6 +205,7 @@ def __init__(self, time_step: float = 0.0, enable_viz: bool = False) -> None: # Tracking data self._robots: dict[WorldRobotID, _RobotData] = {} + self._planning_groups = PlanningGroupRegistry() self._obstacles: dict[str, _ObstacleData] = {} self._robot_counter = 0 self._obstacle_counter = 0 @@ -202,6 +224,9 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: """Add a robot to the world. Returns robot_id. Same model_path + base_pose reuses the model instance (e.g. two arms in one URDF). + base_pose/base_link/end_effector_link remain compatibility fields for + placement and robot-scoped helpers; group-aware planning should use + planning group base/tip links. """ if self._finalized: raise RuntimeError("Cannot add robot after world is finalized") @@ -215,9 +240,7 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: self._validate_joints(config, model_instance) - ee_frame = self._plant.GetBodyByName( - config.end_effector_link, model_instance - ).body_frame() + ee_frame = self._legacy_ee_frame(config, model_instance) base_frame = self._plant.GetBodyByName(config.base_link, model_instance).body_frame() # Preview (yellow ghost) — always a separate instance per robot @@ -235,10 +258,17 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: base_frame=base_frame, preview_model_instance=preview_model_instance, ) + self._planning_groups.add_robot(config) logger.info(f"Added robot '{robot_id}' ({config.name})") return robot_id + def _legacy_ee_frame(self, config: RobotModelConfig, model_instance: Any) -> Any | None: + """Resolve compatibility robot-scoped EE frame, if available.""" + if config.end_effector_link is None: + return None + return self._plant.GetBodyByName(config.end_effector_link, model_instance).body_frame() + def _load_model(self, config: RobotModelConfig) -> Any: """Load robot model (URDF/xacro/MJCF) and return model instance.""" original_path = config.model_path.resolve() @@ -255,6 +285,9 @@ def _load_model(self, config: RobotModelConfig) -> Any: package_paths=config.package_paths, xacro_args=config.xacro_args, convert_meshes=config.auto_convert_meshes, + strip_world_joint_child_link=config.base_link + if config.strip_model_world_joint + else None, ) prepared_path_obj = Path(prepared_path) @@ -317,6 +350,18 @@ def get_robot_config(self, robot_id: WorldRobotID) -> RobotModelConfig: raise KeyError(f"Robot '{robot_id}' not found") return self._robots[robot_id].config + def _get_robot_data_by_name(self, robot_name: str) -> _RobotData: + matches = [ + robot_data + for robot_data in self._robots.values() + if robot_data.config.name == robot_name + ] + if not matches: + raise KeyError(f"Robot '{robot_name}' not found for planning group resolution") + if len(matches) > 1: + raise ValueError(f"Robot name '{robot_name}' is not unique in planning world") + return matches[0] + def get_joint_limits( self, robot_id: WorldRobotID ) -> tuple[NDArray[np.float64], NDArray[np.float64]]: @@ -686,7 +731,7 @@ def finalize(self) -> None: self.publish_visualization() # Hide all preview robots initially for robot_id in self._robots: - self.hide_preview(robot_id) + self._hide_preview_robot(robot_id) @property def is_finalized(self) -> bool: @@ -768,8 +813,7 @@ def sync_from_joint_state(self, robot_id: WorldRobotID, joint_state: JointState) if not self._finalized or self._plant_context is None: return # Silently ignore before finalization - # Extract positions as numpy array for internal use - positions = np.array(joint_state.position, dtype=np.float64) + positions = self._positions_for_robot_state(robot_id, joint_state) with self._lock: self._set_positions_internal(self._plant_context, robot_id, positions) @@ -787,8 +831,7 @@ def set_joint_state( if not self._finalized: raise RuntimeError("World must be finalized first") - # Extract positions as numpy array for internal use - positions = np.array(joint_state.position, dtype=np.float64) + positions = self._positions_for_robot_state(robot_id, joint_state) # Get plant context from diagram context plant_ctx = self._diagram.GetMutableSubsystemContext(self._plant, ctx) @@ -809,6 +852,37 @@ def _set_positions_internal( self._plant.SetPositions(plant_ctx, full_positions) + def _positions_for_robot_state( + self, robot_id: WorldRobotID, joint_state: JointState + ) -> NDArray[np.float64]: + if robot_id not in self._robots: + raise KeyError(f"Robot '{robot_id}' not found") + robot_data = self._robots[robot_id] + local_joint_names = robot_data.config.joint_names + + if not joint_state.name: + if len(joint_state.position) != len(local_joint_names): + raise ValueError( + f"JointState position length {len(joint_state.position)} does not match " + f"robot {robot_data.config.name} joint count {len(local_joint_names)}" + ) + return np.array(joint_state.position, dtype=np.float64) + + state_by_local_name: dict[str, float] = {} + if len(joint_state.name) != len(joint_state.position): + raise ValueError("JointState name and position lengths must match") + + assert_local_joint_names(joint_state.name) + for name, position in zip(joint_state.name, joint_state.position, strict=False): + state_by_local_name[name] = float(position) + + missing = [name for name in local_joint_names if name not in state_by_local_name] + if missing: + raise ValueError( + f"JointState for robot {robot_data.config.name} is missing joints: {missing}" + ) + return np.array([state_by_local_name[name] for name in local_joint_names], dtype=np.float64) + def get_joint_state(self, ctx: Context, robot_id: WorldRobotID) -> JointState: """Get robot joint state from given context.""" if not self._finalized: @@ -818,11 +892,19 @@ def get_joint_state(self, ctx: Context, robot_id: WorldRobotID) -> JointState: raise KeyError(f"Robot '{robot_id}' not found") robot_data = self._robots[robot_id] + if robot_data.ee_frame is None: + raise ValueError( + f"Robot '{robot_id}' has no robot-scoped end-effector link; " + "use get_group_pose() with an explicit planning group ID" + ) plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) full_positions = self._plant.GetPositions(plant_ctx) positions = [float(full_positions[idx]) for idx in robot_data.joint_indices] - return JointState(name=robot_data.config.joint_names, position=positions) + return JointState( + name=list(robot_data.config.joint_names), + position=positions, + ) # Collision Checking (context-based) @@ -903,8 +985,28 @@ def check_edge_collision_free( # Forward Kinematics (context-based) + def get_group_pose(self, ctx: Context, group_id: PlanningGroupID) -> PoseStamped: + """Get pose for a planning group's target frame.""" + if not self._finalized: + raise RuntimeError("World must be finalized first") + + group = self._planning_groups.get(group_id) + if group.tip_link is None: + raise ValueError(f"Planning group '{group_id}' has no pose target frame") + + robot_data = self._get_robot_data_by_name(group.robot_name) + plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) + + try: + tip_body = self._plant.GetBodyByName(group.tip_link, robot_data.model_instance) + except RuntimeError: + raise KeyError(f"Planning group '{group_id}' target link '{group.tip_link}' not found") + + tip_pose = self._plant.EvalBodyPoseInWorld(plant_ctx, tip_body) + return _pose_stamped_from_drake_transform(tip_pose) + def get_ee_pose(self, ctx: Context, robot_id: WorldRobotID) -> PoseStamped: - """Get end-effector pose.""" + """Get pose for a robot's compatibility end-effector frame.""" if not self._finalized: raise RuntimeError("World must be finalized first") @@ -912,20 +1014,16 @@ def get_ee_pose(self, ctx: Context, robot_id: WorldRobotID) -> PoseStamped: raise KeyError(f"Robot '{robot_id}' not found") robot_data = self._robots[robot_id] + if robot_data.ee_frame is None: + raise ValueError( + f"Robot '{robot_id}' has no robot-scoped end-effector link; " + "use get_group_jacobian() with an explicit planning group ID" + ) plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) ee_body = robot_data.ee_frame.body() X_WE = self._plant.EvalBodyPoseInWorld(plant_ctx, ee_body) - - # Extract position and quaternion from Drake transform - pos = X_WE.translation() - quat = X_WE.rotation().ToQuaternion() # Drake returns [w, x, y, z] - - return PoseStamped( - frame_id="world", - position=[float(pos[0]), float(pos[1]), float(pos[2])], - orientation=[float(quat.x()), float(quat.y()), float(quat.z()), float(quat.w())], - ) + return _pose_stamped_from_drake_transform(X_WE) def get_link_pose( self, ctx: Context, robot_id: WorldRobotID, link_name: str @@ -951,7 +1049,7 @@ def get_link_pose( return result # type: ignore[no-any-return] def get_jacobian(self, ctx: Context, robot_id: WorldRobotID) -> NDArray[np.float64]: - """Get geometric Jacobian (6 x n_joints). + """Get robot-scoped geometric Jacobian for the compatibility EE frame. Rows: [vx, vy, vz, wx, wy, wz] (linear, then angular) """ @@ -986,6 +1084,52 @@ def get_jacobian(self, ctx: Context, robot_id: WorldRobotID) -> NDArray[np.float return J_reordered + def get_group_jacobian(self, ctx: Context, group_id: PlanningGroupID) -> NDArray[np.float64]: + """Get geometric Jacobian for a planning group's target frame. + + Rows: [vx, vy, vz, wx, wy, wz] (linear, then angular). Columns follow + the resolved planning group's joint order. + """ + if not self._finalized: + raise RuntimeError("World must be finalized first") + + group = self._planning_groups.get(group_id) + if group.tip_link is None: + raise ValueError(f"Planning group '{group_id}' has no pose target frame") + + robot_data = self._get_robot_data_by_name(group.robot_name) + plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) + + try: + tip_body = self._plant.GetBodyByName(group.tip_link, robot_data.model_instance) + except RuntimeError: + raise KeyError(f"Planning group '{group_id}' target link '{group.tip_link}' not found") + + jacobian_full = self._plant.CalcJacobianSpatialVelocity( + plant_ctx, + JacobianWrtVariable.kQDot, + tip_body.body_frame(), + np.array([0.0, 0.0, 0.0]), # type: ignore[arg-type] + self._plant.world_frame(), + self._plant.world_frame(), + ) + + joint_indices_by_name = dict( + zip(robot_data.config.joint_names, robot_data.joint_indices, strict=False) + ) + jacobian_group = np.zeros((6, len(group.local_joint_names))) + for index, local_joint_name in enumerate(group.local_joint_names): + try: + joint_index = joint_indices_by_name[local_joint_name] + except KeyError: + raise ValueError( + f"Planning group '{group_id}' references non-controllable joint " + f"'{local_joint_name}'" + ) + jacobian_group[:, index] = jacobian_full[:, joint_index] + + return np.vstack([jacobian_group[3:6, :], jacobian_group[0:3, :]]) + # Visualization def initialize_scene(self, scene: PlanningSceneInfo) -> None: @@ -1021,8 +1165,19 @@ def _set_preview_positions( full_positions[idx] = positions[i] self._plant.SetPositions(plant_ctx, full_positions) - def show_preview(self, robot_id: WorldRobotID) -> None: - """Show the preview (yellow ghost) robot in Meshcat.""" + def _preview_robot_ids_for_groups( + self, group_ids: Sequence[PlanningGroupID] + ) -> list[WorldRobotID]: + """Resolve planning groups to stable preview robot IDs.""" + robot_ids: list[WorldRobotID] = [] + for group in self._planning_groups.select(tuple(group_ids)).groups: + robot_id = self._get_robot_data_by_name(group.robot_name).robot_id + if robot_id not in robot_ids: + robot_ids.append(robot_id) + return robot_ids + + def _show_preview_robot(self, robot_id: WorldRobotID) -> None: + """Show one preview (yellow ghost) robot in Meshcat.""" if self._meshcat is None: return robot_data = self._robots.get(robot_id) @@ -1031,8 +1186,13 @@ def show_preview(self, robot_id: WorldRobotID) -> None: model_name = self._plant.GetModelInstanceName(robot_data.preview_model_instance) self._meshcat.SetProperty(f"visualizer/{model_name}", "visible", True) - def hide_preview(self, robot_id: WorldRobotID) -> None: - """Hide the preview (yellow ghost) robot in Meshcat.""" + def show_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Show preview robots affected by planning groups.""" + for robot_id in self._preview_robot_ids_for_groups(group_ids): + self._show_preview_robot(robot_id) + + def _hide_preview_robot(self, robot_id: WorldRobotID) -> None: + """Hide one preview (yellow ghost) robot in Meshcat.""" if self._meshcat is None: return robot_data = self._robots.get(robot_id) @@ -1041,32 +1201,63 @@ def hide_preview(self, robot_id: WorldRobotID) -> None: model_name = self._plant.GetModelInstanceName(robot_data.preview_model_instance) self._meshcat.SetProperty(f"visualizer/{model_name}", "visible", False) - def animate_path( + def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: + """Hide preview robots affected by planning groups.""" + for robot_id in self._preview_robot_ids_for_groups(group_ids): + self._hide_preview_robot(robot_id) + + def _preview_positions_for_waypoint( self, robot_id: WorldRobotID, - path: JointPath, - duration: float = 3.0, - ) -> None: - """Animate a path using the preview (yellow ghost) robot. + selected_positions_by_name: dict[str, float], + current_positions: NDArray[np.float64], + ) -> NDArray[np.float64]: + """Build full local preview positions for one robot from selected globals.""" + robot_data = self._robots[robot_id] + positions = current_positions.copy() + for index, local_name in enumerate(robot_data.config.joint_names): + global_name = make_global_joint_name(robot_data.config.name, local_name) + if global_name in selected_positions_by_name: + positions[index] = selected_positions_by_name[global_name] + return positions + + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: + """Animate a generated plan using preview (yellow ghost) robots. The preview stays visible after animation completes. """ - if self._meshcat is None or len(path) < 2: + if self._meshcat is None or len(plan.path) < 2: return - robot_data = self._robots.get(robot_id) - if robot_data is None or robot_data.preview_model_instance is None: + robot_ids = [ + robot_id + for robot_id in self._preview_robot_ids_for_groups(plan.group_ids) + if self._robots[robot_id].preview_model_instance is not None + ] + if not robot_ids: return import time - self.show_preview(robot_id) - dt = duration / (len(path) - 1) - for joint_state in path: - positions = np.array(joint_state.position, dtype=np.float64) + self.show_preview(plan.group_ids) + dt = duration / (len(plan.path) - 1) + for joint_state in plan.path: + selected_positions_by_name = dict( + zip(joint_state.name, joint_state.position, strict=True) + ) with self._lock: assert self._plant_context is not None - self._set_preview_positions(self._plant_context, robot_id, positions) + for robot_id in robot_ids: + robot_data = self._robots[robot_id] + current_positions = self._plant.GetPositions( + self._plant_context, robot_data.model_instance + ) + positions = self._preview_positions_for_waypoint( + robot_id, + selected_positions_by_name, + np.array(current_positions, dtype=np.float64), + ) + self._set_preview_positions(self._plant_context, robot_id, positions) self.publish_visualization() time.sleep(dt) diff --git a/dimos/manipulation/planning/world/test_drake_world_planning_groups.py b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py new file mode 100644 index 0000000000..edaf5cacfe --- /dev/null +++ b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py @@ -0,0 +1,210 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Tests for DrakeWorld planning group name/world resolution.""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +import pytest + +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition +from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.world.drake_world import DrakeWorld, _RobotData +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState + + +def _pose() -> PoseStamped: + return PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]) + + +def _config( + name: str, + joint_names: list[str], + groups: list[PlanningGroupDefinition], +) -> RobotModelConfig: + return RobotModelConfig( + name=name, + model_path=Path("robot.urdf"), + base_pose=_pose(), + joint_names=joint_names, + end_effector_link="tool0", + base_link="base_link", + planning_groups=groups, + ) + + +def _world(*configs: RobotModelConfig) -> DrakeWorld: + world = DrakeWorld.__new__(DrakeWorld) + world._robots = { + f"robot_{index}": _RobotData( + robot_id=f"robot_{index}", + config=config, + model_instance=None, + joint_indices=[], + ee_frame=None, + base_frame=None, + ) + for index, config in enumerate(configs, start=1) + } + world._planning_groups = PlanningGroupRegistry(configs) + return world + + +def _arm_group(*joint_names: str) -> PlanningGroupDefinition: + return PlanningGroupDefinition( + name="arm", + joint_names=joint_names, + base_link="base_link", + tip_link="tool0", + source="srdf", + ) + + +def test_planning_group_registry_returns_stable_ids_and_global_joint_names() -> None: + config = _config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")]) + registry = PlanningGroupRegistry([config]) + + groups = registry.list() + + assert len(groups) == 1 + assert groups[0].id == "left/arm" + assert groups[0].robot_name == "left" + assert groups[0].group_name == "arm" + assert groups[0].joint_names == ("left/joint1", "left/joint2") + assert groups[0].local_joint_names == ("joint1", "joint2") + + +def test_robot_model_config_allows_planning_groups_without_robot_scoped_ee() -> None: + config = RobotModelConfig( + name="left", + model_path=Path("robot.urdf"), + joint_names=["joint1"], + planning_groups=[_arm_group("joint1")], + ) + registry = PlanningGroupRegistry([config]) + + groups = registry.list() + + assert config.end_effector_link is None + assert groups[0].id == "left/arm" + + +def test_duplicate_local_joint_names_across_robots_are_disambiguated() -> None: + registry = PlanningGroupRegistry( + [ + _config("left", ["joint1"], [_arm_group("joint1")]), + _config("right", ["joint1"], [_arm_group("joint1")]), + ] + ) + + groups = registry.list() + + assert [group.id for group in groups] == ["left/arm", "right/arm"] + assert [group.joint_names for group in groups] == [("left/joint1",), ("right/joint1",)] + + +def test_planning_group_selection_returns_ordered_global_joint_names() -> None: + registry = PlanningGroupRegistry( + [ + _config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")]), + _config("right", ["joint1", "joint2"], [_arm_group("joint2")]), + ] + ) + + selection = registry.select(("left/arm", "right/arm")) + + assert list(selection.group_ids) == ["left/arm", "right/arm"] + assert list(selection.robot_names) == ["left", "right"] + assert [group.joint_names for group in selection.groups] == [ + ("left/joint1", "left/joint2"), + ("right/joint2",), + ] + assert list(selection.joint_names) == ["left/joint1", "left/joint2", "right/joint2"] + assert [group.local_joint_names for group in selection.groups] == [ + ("joint1", "joint2"), + ("joint2",), + ] + + +def test_planning_group_registry_unknown_group_raises_key_error() -> None: + registry = PlanningGroupRegistry([_config("left", ["joint1"], [_arm_group("joint1")])]) + + with pytest.raises(KeyError, match="Unknown planning group ID: left/gripper"): + registry.select(("left/gripper",)) + + +def test_planning_group_selection_overlapping_same_robot_groups_raise_value_error() -> None: + registry = PlanningGroupRegistry( + [ + _config( + "left", + ["joint1", "joint2"], + [ + _arm_group("joint1", "joint2"), + PlanningGroupDefinition( + name="wrist", + joint_names=("joint2",), + base_link="link1", + tip_link="tool0", + ), + ], + ) + ] + ) + + with pytest.raises(ValueError, match="overlap.*left/joint2"): + registry.select(("left/arm", "left/wrist")) + + +def test_positions_for_robot_state_accepts_local_joint_names_in_config_order() -> None: + world = _world(_config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")])) + joint_state = JointState({"name": ["joint2", "joint1"], "position": [2.0, 1.0]}) + + positions = world._positions_for_robot_state("robot_1", joint_state) + + np.testing.assert_allclose(positions, np.array([1.0, 2.0])) + + +def test_positions_for_robot_state_rejects_global_joint_names() -> None: + world = _world(_config("left", ["joint1", "joint2"], [_arm_group("joint1", "joint2")])) + joint_state = JointState({"name": ["left/joint2", "left/joint1"], "position": [2.0, 1.0]}) + + with pytest.raises(ValueError, match="Invalid local joint name: 'left/joint2'"): + world._positions_for_robot_state("robot_1", joint_state) + + +def test_group_pose_rejects_group_without_target_frame() -> None: + world = _world( + _config( + "left", + ["joint1"], + [ + PlanningGroupDefinition( + name="waist", + joint_names=("joint1",), + base_link="base_link", + tip_link=None, + ) + ], + ) + ) + world._finalized = True + + with pytest.raises(ValueError, match="left/waist.*no pose target frame"): + world.get_group_pose(None, "left/waist") diff --git a/dimos/manipulation/test_manipulation_module.py b/dimos/manipulation/test_manipulation_module.py index f8e914e3b3..f3ff29ace0 100644 --- a/dimos/manipulation/test_manipulation_module.py +++ b/dimos/manipulation/test_manipulation_module.py @@ -69,15 +69,6 @@ def _get_xarm7_config() -> RobotModelConfig: auto_convert_meshes=True, max_velocity=1.0, max_acceleration=2.0, - joint_name_mapping={ - "arm/joint1": "joint1", - "arm/joint2": "joint2", - "arm/joint3": "joint3", - "arm/joint4": "joint4", - "arm/joint5": "joint5", - "arm/joint6": "joint6", - "arm/joint7": "joint7", - }, coordinator_task_name="traj_arm", ) @@ -92,13 +83,13 @@ def joint_state_zeros(): """Create a JointState message with zeros for XArm7.""" return JointState( name=[ - "arm/joint1", - "arm/joint2", - "arm/joint3", - "arm/joint4", - "arm/joint5", - "arm/joint6", - "arm/joint7", + "test_arm/joint1", + "test_arm/joint2", + "test_arm/joint3", + "test_arm/joint4", + "test_arm/joint5", + "test_arm/joint6", + "test_arm/joint7", ], position=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], velocity=[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], @@ -160,11 +151,8 @@ def test_plan_to_joints(self, module, joint_state_zeros): assert success is True assert module._state == ManipulationState.COMPLETED assert module.has_planned_path() is True - - assert "test_arm" in module._planned_trajectories - traj = module._planned_trajectories["test_arm"] - assert len(traj.points) > 1 - assert traj.duration > 0 + assert module._last_plan is not None + assert len(module._last_plan.path) > 1 def test_add_and_remove_obstacle(self, module, joint_state_zeros): """Test adding and removing obstacles.""" @@ -191,7 +179,6 @@ def test_robot_info(self, module): assert len(info["joint_names"]) == 7 assert info["end_effector_link"] == "link7" assert info["coordinator_task_name"] == "traj_arm" - assert info["has_joint_name_mapping"] is True def test_ee_pose(self, module, joint_state_zeros): """Test getting end-effector pose.""" @@ -204,20 +191,21 @@ def test_ee_pose(self, module, joint_state_zeros): assert hasattr(pose, "y") assert hasattr(pose, "z") - def test_trajectory_name_translation(self, module, joint_state_zeros): - """Test that trajectory joint names are translated for coordinator.""" + def test_planned_trajectory_uses_global_joint_names(self, module, joint_state_zeros): + """Test that planned trajectory joint names are global for coordinator.""" module._on_joint_state(joint_state_zeros) success = module.plan_to_joints(JointState(position=[0.05] * 7)) assert success is True - traj = module._planned_trajectories["test_arm"] - robot_config = module._robots["test_arm"][1] + mock_client = MagicMock() + mock_client.task_invoke.return_value = True + module._coordinator_client = mock_client - translated = module._translate_trajectory_to_coordinator(traj, robot_config) + assert module.execute() is True - for name in translated.joint_names: - assert name.startswith("arm_") # Should have arm_ prefix + trajectory = mock_client.task_invoke.call_args.args[2]["trajectory"] + assert trajectory.joint_names == [f"test_arm/joint{i}" for i in range(1, 8)] @pytest.mark.skipif(not _drake_available(), reason="Drake not installed") @@ -251,8 +239,7 @@ def test_execute_with_mock_coordinator(self, module, joint_state_zeros): assert method_name == "execute" trajectory = kwargs["trajectory"] assert len(trajectory.points) > 1 - # Joint names should be translated - assert all(n.startswith("arm_") for n in trajectory.joint_names) + assert trajectory.joint_names == [f"test_arm/joint{i}" for i in range(1, 8)] def test_execute_rejected_by_coordinator(self, module, joint_state_zeros): """Test handling of coordinator rejection.""" diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index 4c83e9c3ad..3aeae3fd92 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -28,11 +28,16 @@ ManipulationModuleConfig, ManipulationState, ) +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.manipulation.planning.spec.enums import IKStatus -from dimos.manipulation.planning.spec.models import IKResult, PlanningSceneInfo +from dimos.manipulation.planning.spec.enums import IKStatus, PlanningStatus +from dimos.manipulation.planning.spec.models import ( + GeneratedPlan, + IKResult, + PlanningSceneInfo, +) from dimos.manipulation.planning.spec.protocols import VisualizationSpec from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -60,8 +65,8 @@ def robot_config(): @pytest.fixture -def robot_config_with_mapping(): - """Create a robot config with joint name mapping (dual-arm scenario).""" +def left_robot_config(): + """Create a robot config for a scoped left arm.""" return RobotModelConfig( name="left_arm", model_path=Path("/path/to/robot.urdf"), @@ -69,11 +74,6 @@ def robot_config_with_mapping(): joint_names=["joint1", "joint2", "joint3"], end_effector_link="link_tcp", base_link="link_base", - joint_name_mapping={ - "left/joint1": "joint1", - "left/joint2": "joint2", - "left/joint3": "joint3", - }, coordinator_task_name="traj_left", ) @@ -101,12 +101,11 @@ def __init__(self) -> None: self._error_message = "" self._planning_epoch = 0 self._robots = {} - self._planned_paths = {} - self._planned_trajectories = {} self._world_monitor = None self._planner = None self._kinematics = None self._coordinator_client = None + self._last_plan = None self.config = MagicMock(planning_timeout=10.0) @@ -165,19 +164,20 @@ def test_begin_planning_state_checks(self, robot_config): module = _make_module() module._world_monitor = MagicMock() module._robots = {"test_arm": ("robot_id", robot_config, MagicMock())} + group_ids = ("test_arm/manipulator",) # From IDLE - OK module._state = ManipulationState.IDLE - assert module._begin_planning() == ("test_arm", "robot_id") + assert module._begin_planning(group_ids) == group_ids assert module._state == ManipulationState.PLANNING # From COMPLETED - OK module._state = ManipulationState.COMPLETED - assert module._begin_planning() == ("test_arm", "robot_id") + assert module._begin_planning(group_ids) == group_ids # From EXECUTING - Fail module._state = ManipulationState.EXECUTING - assert module._begin_planning() is None + assert module._begin_planning(group_ids) is None class TestRobotSelection: @@ -324,7 +324,6 @@ def test_solve_ik_rpc_calls_configured_backend(self, robot_config): assert result is expected assert module._state == ManipulationState.COMPLETED - assert module._planned_paths == {} module._kinematics.solve.assert_called_once() _, kwargs = module._kinematics.solve.call_args assert kwargs["world"] is module._world_monitor.world @@ -373,27 +372,6 @@ def test_solve_ik_rpc_uses_explicit_seed(self, robot_config): module._world_monitor.get_current_joint_state.assert_not_called() -class TestJointNameTranslation: - """Test trajectory joint name translation for coordinator.""" - - def test_no_mapping_returns_original(self, robot_config, simple_trajectory): - """Without mapping, trajectory is returned unchanged.""" - module = _make_module() - - result = module._translate_trajectory_to_coordinator(simple_trajectory, robot_config) - assert result is simple_trajectory # Same object - - def test_mapping_translates_names(self, robot_config_with_mapping, simple_trajectory): - """With mapping, joint names are translated.""" - module = _make_module() - - result = module._translate_trajectory_to_coordinator( - simple_trajectory, robot_config_with_mapping - ) - assert result.joint_names == ["left/joint1", "left/joint2", "left/joint3"] - assert len(result.points) == 2 # Points preserved - - class TestExecute: """Test coordinator execution.""" @@ -401,13 +379,11 @@ def test_execute_requires_trajectory(self, robot_config): """Execute fails without planned trajectory.""" module = _make_module() module._robots = {"test_arm": ("id", robot_config, MagicMock())} - module._planned_trajectories = {} assert module.execute() is False def test_execute_requires_task_name(self): """Execute fails without coordinator_task_name.""" - module = _make_module() config_no_task = RobotModelConfig( name="arm", model_path=Path("/path"), @@ -415,16 +391,47 @@ def test_execute_requires_task_name(self): joint_names=["j1"], end_effector_link="ee", ) - module._robots = {"arm": ("id", config_no_task, MagicMock())} - module._planned_trajectories = {"arm": MagicMock()} + module = _make_module_with_monitor(config_no_task) + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("arm", "manipulator", ["j1"])] + ) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1"], position=[0.0] + ) + module._last_plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j1"], position=[1.0])], + status=PlanningStatus.SUCCESS, + ) assert module.execute() is False def test_execute_success(self, robot_config, simple_trajectory): """Successful execute calls coordinator via task_invoke.""" - module = _make_module() - module._robots = {"test_arm": ("id", robot_config, MagicMock())} - module._planned_trajectories = {"test_arm": simple_trajectory} + module = _make_module_with_monitor(robot_config) + generator = MagicMock() + generator.generate.return_value = simple_trajectory + module._robots = {"test_arm": ("id", robot_config, generator)} + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("test_arm", "manipulator", ["joint1", "joint2", "joint3"])] + ) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["joint1", "joint2", "joint3"], position=[0.0, 0.0, 0.0] + ) + module._last_plan = GeneratedPlan( + group_ids=("test_arm/manipulator",), + path=[ + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.0, 0.0, 0.0], + ), + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.5, 0.5, 0.5], + ), + ], + status=PlanningStatus.SUCCESS, + ) mock_client = MagicMock() mock_client.task_invoke.return_value = True @@ -432,15 +439,42 @@ def test_execute_success(self, robot_config, simple_trajectory): assert module.execute() is True assert module._state == ManipulationState.COMPLETED - mock_client.task_invoke.assert_called_once_with( - "traj_arm", "execute", {"trajectory": simple_trajectory} - ) + mock_client.task_invoke.assert_called_once() + assert mock_client.task_invoke.call_args.args[:2] == ("traj_arm", "execute") + trajectory = mock_client.task_invoke.call_args.args[2]["trajectory"] + assert trajectory.joint_names == [ + "test_arm/joint1", + "test_arm/joint2", + "test_arm/joint3", + ] + assert trajectory.points == simple_trajectory.points def test_execute_rejected(self, robot_config, simple_trajectory): """Rejected execution sets FAULT state.""" - module = _make_module() - module._robots = {"test_arm": ("id", robot_config, MagicMock())} - module._planned_trajectories = {"test_arm": simple_trajectory} + module = _make_module_with_monitor(robot_config) + generator = MagicMock() + generator.generate.return_value = simple_trajectory + module._robots = {"test_arm": ("id", robot_config, generator)} + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("test_arm", "manipulator", ["joint1", "joint2", "joint3"])] + ) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["joint1", "joint2", "joint3"], position=[0.0, 0.0, 0.0] + ) + module._last_plan = GeneratedPlan( + group_ids=("test_arm/manipulator",), + path=[ + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.0, 0.0, 0.0], + ), + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.5, 0.5, 0.5], + ), + ], + status=PlanningStatus.SUCCESS, + ) mock_client = MagicMock() mock_client.task_invoke.return_value = False @@ -449,21 +483,47 @@ def test_execute_rejected(self, robot_config, simple_trajectory): assert module.execute() is False assert module._state == ManipulationState.FAULT + def test_execute_times_out_when_coordinator_rpc_does_not_respond( + self, robot_config, simple_trajectory + ): + """Coordinator RPC timeout fails execution instead of hanging silently.""" + module = _make_module_with_monitor(robot_config) + module.config.coordinator_rpc_timeout = 0.01 + generator = MagicMock() + generator.generate.return_value = simple_trajectory + module._robots = {"test_arm": ("id", robot_config, generator)} + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("test_arm", "manipulator", ["joint1", "joint2", "joint3"])] + ) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["joint1", "joint2", "joint3"], position=[0.0, 0.0, 0.0] + ) + module._last_plan = GeneratedPlan( + group_ids=("test_arm/manipulator",), + path=[ + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.0, 0.0, 0.0], + ), + JointState( + name=["test_arm/joint1", "test_arm/joint2", "test_arm/joint3"], + position=[0.5, 0.5, 0.5], + ), + ], + status=PlanningStatus.SUCCESS, + ) + mock_client = MagicMock() + mock_client.remote_name = "ControlCoordinator" + mock_client._unsub_fns = [] + mock_client.rpc.call_sync.side_effect = TimeoutError("no response") + module._coordinator_client = mock_client -class TestRobotModelConfigMapping: - """Test RobotModelConfig joint name mapping helpers.""" - - def test_bidirectional_mapping(self, robot_config_with_mapping): - """Test URDF <-> coordinator name translation.""" - config = robot_config_with_mapping - - # Coordinator -> URDF - assert config.get_urdf_joint_name("left/joint1") == "joint1" - assert config.get_urdf_joint_name("unknown") == "unknown" + assert module.execute() is False - # URDF -> Coordinator - assert config.get_coordinator_joint_name("joint1") == "left/joint1" - assert config.get_coordinator_joint_name("unknown") == "unknown" + assert module._state == ManipulationState.FAULT + assert "timed out" in module._error_message + mock_client.rpc.call_sync.assert_called_once() + mock_client.task_invoke.assert_not_called() def _make_module_with_monitor(*configs: RobotModelConfig) -> ManipulationModule: @@ -474,6 +534,12 @@ def _make_module_with_monitor(*configs: RobotModelConfig) -> ManipulationModule: for config in configs: robot_id = f"robot_{config.name}" module._robots[config.name] = (robot_id, config, MagicMock()) + module._world_monitor.planning_groups = _FakePlanningGroups( + [ + _make_global_group(config.name, "manipulator", list(config.joint_names)) + for config in configs + ] + ) return module @@ -481,19 +547,84 @@ def _make_joint_state(positions: list[float], name: list[str] | None = None) -> return JointState(name=name or [f"j{i}" for i in range(len(positions))], position=positions) -def _make_path(*points: list[float]) -> list[JointState]: - return [_make_joint_state(list(point)) for point in points] +def _make_robot_config( + name: str, + joints: list[str], + task_name: str, +) -> RobotModelConfig: + return RobotModelConfig( + name=name, + model_path=Path("/path/to/robot.urdf"), + base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), + joint_names=joints, + end_effector_link="ee", + base_link="base", + coordinator_task_name=task_name, + ) + +def _make_global_group(robot_name: str, group_name: str, joints: list[str]) -> PlanningGroup: + return PlanningGroup( + id=f"{robot_name}/{group_name}", + robot_name=robot_name, + group_name=group_name, + joint_names=tuple(f"{robot_name}/{joint}" for joint in joints), + local_joint_names=tuple(joints), + base_link="base", + tip_link="ee", + ) -def _make_trajectory(*points: tuple[float, list[float]]) -> JointTrajectory: - joint_names = [f"j{i}" for i in range(len(points[0][1]))] if points else [] - return JointTrajectory( - joint_names=joint_names, + +class _FakePlanningGroups: + def __init__(self, groups: list[PlanningGroup]) -> None: + self._groups = {group.id: group for group in groups} + + def get(self, group_id: str) -> PlanningGroup: + return self._groups[group_id] + + def select(self, group_ids: tuple[str, ...]) -> PlanningGroupSelection: + return PlanningGroupSelection.from_groups( + tuple(self._groups[group_id] for group_id in group_ids) + ) + + def groups_for_robot(self, robot_name: str) -> tuple[PlanningGroup, ...]: + return tuple(group for group in self._groups.values() if group.robot_name == robot_name) + + def default_group_id_for_robot(self, robot_name: str) -> str | None: + group_id = f"{robot_name}/manipulator" + return group_id if group_id in self._groups else None + + def primary_pose_group_id_for_robot(self, robot_name: str) -> str | None: + for group in self.groups_for_robot(robot_name): + if group.has_pose_target: + return group.id + return None + + +def _make_generated_plan(group_ids: tuple[str, ...], *points: list[float]) -> GeneratedPlan: + return GeneratedPlan( + group_ids=group_ids, + path=[ + JointState( + name=["left/j1", "left/j2", "right/j1"], + position=list(point), + ) + for point in points + ], + status=PlanningStatus.SUCCESS, + ) + + +def _trajectory_generator() -> MagicMock: + generator = MagicMock() + generator.generate.side_effect = lambda positions: JointTrajectory( + joint_names=[], points=[ - TrajectoryPoint(time_from_start=time_from_start, positions=positions) - for time_from_start, positions in points + TrajectoryPoint(time_from_start=float(index), positions=list(position)) + for index, position in enumerate(positions) ], ) + return generator def _make_world_monitor_with_viz(viz: VisualizationSpec | None) -> WorldMonitor: @@ -508,9 +639,9 @@ class FakeVisualization: def __init__(self) -> None: self.close_count = 0 self.published = False - self.preview_shown: list[str] = [] - self.preview_hidden: list[str] = [] - self.animations: list[tuple[str, list[JointState], float]] = [] + self.preview_shown: list[tuple[str, ...]] = [] + self.preview_hidden: list[tuple[str, ...]] = [] + self.animations: list[tuple[GeneratedPlan, float]] = [] def initialize_scene(self, scene: PlanningSceneInfo) -> None: pass @@ -521,14 +652,14 @@ def get_visualization_url(self) -> str | None: def publish_visualization(self, ctx: object | None = None) -> None: self.published = True - def show_preview(self, robot_id: str) -> None: - self.preview_shown.append(robot_id) + def show_preview(self, group_ids: Sequence[str]) -> None: + self.preview_shown.append(tuple(group_ids)) - def hide_preview(self, robot_id: str) -> None: - self.preview_hidden.append(robot_id) + def hide_preview(self, group_ids: Sequence[str]) -> None: + self.preview_hidden.append(tuple(group_ids)) - def animate_path(self, robot_id: str, path: list[JointState], duration: float = 3.0) -> None: - self.animations.append((robot_id, path, duration)) + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: + self.animations.append((plan, duration)) def close(self) -> None: self.close_count += 1 @@ -537,12 +668,12 @@ def close(self) -> None: class TestOnJointState: """Test _on_joint_state routing, splitting, and init capture.""" - def test_routes_positions_to_monitor(self, robot_config_with_mapping): + def test_routes_positions_to_monitor(self, left_robot_config): """Joint positions from aggregated message are routed to the correct monitor.""" - module = _make_module_with_monitor(robot_config_with_mapping) + module = _make_module_with_monitor(left_robot_config) msg = JointState( - name=["left/joint1", "left/joint2", "left/joint3"], + name=["left_arm/joint1", "left_arm/joint2", "left_arm/joint3"], position=[0.1, 0.2, 0.3], velocity=[1.0, 2.0, 3.0], ) @@ -552,13 +683,14 @@ def test_routes_positions_to_monitor(self, robot_config_with_mapping): module._world_monitor.on_joint_state.assert_called_once() call_args = module._world_monitor.on_joint_state.call_args sub_msg = call_args[0][0] + assert sub_msg.name == ["joint1", "joint2", "joint3"] assert sub_msg.position == [0.1, 0.2, 0.3] assert sub_msg.velocity == [1.0, 2.0, 3.0] assert call_args[1]["robot_id"] == "robot_left_arm" - def test_skips_robot_with_missing_joints(self, robot_config_with_mapping): + def test_skips_robot_with_missing_joints(self, left_robot_config): """Robots whose joints are absent from the message are skipped.""" - module = _make_module_with_monitor(robot_config_with_mapping) + module = _make_module_with_monitor(left_robot_config) # Message has none of left_arm's joints msg = JointState( @@ -569,12 +701,12 @@ def test_skips_robot_with_missing_joints(self, robot_config_with_mapping): module._world_monitor.on_joint_state.assert_not_called() - def test_captures_init_joints_on_first_call(self, robot_config_with_mapping): + def test_captures_init_joints_on_first_call(self, left_robot_config): """First joint state is stored as init joints; subsequent calls don't overwrite.""" - module = _make_module_with_monitor(robot_config_with_mapping) + module = _make_module_with_monitor(left_robot_config) first_msg = JointState( - name=["left/joint1", "left/joint2", "left/joint3"], + name=["left_arm/joint1", "left_arm/joint2", "left_arm/joint3"], position=[0.1, 0.2, 0.3], ) module._on_joint_state(first_msg) @@ -583,7 +715,7 @@ def test_captures_init_joints_on_first_call(self, robot_config_with_mapping): # Second call should NOT overwrite second_msg = JointState( - name=["left/joint1", "left/joint2", "left/joint3"], + name=["left_arm/joint1", "left_arm/joint2", "left_arm/joint3"], position=[0.9, 0.8, 0.7], ) module._on_joint_state(second_msg) @@ -598,7 +730,6 @@ def test_multi_robot_splits_correctly(self): joint_names=["j1", "j2"], end_effector_link="ee", base_link="base", - joint_name_mapping={"left/j1": "j1", "left/j2": "j2"}, coordinator_task_name="traj_left", ) right_config = RobotModelConfig( @@ -608,7 +739,6 @@ def test_multi_robot_splits_correctly(self): joint_names=["j1", "j2"], end_effector_link="ee", base_link="base", - joint_name_mapping={"right/j1": "j1", "right/j2": "j2"}, coordinator_task_name="traj_right", ) module = _make_module_with_monitor(left_config, right_config) @@ -632,15 +762,15 @@ def test_multi_robot_splits_correctly(self): assert calls["robot_left"].velocity == [0.1, 0.2] assert calls["robot_right"].velocity == [0.3, 0.4] - def test_no_monitor_returns_early(self, robot_config_with_mapping): + def test_no_monitor_returns_early(self, left_robot_config): """When world_monitor is None, _on_joint_state returns without error.""" module = _make_module() - module._robots = {"left_arm": ("id", robot_config_with_mapping, MagicMock())} + module._robots = {"left_arm": ("id", left_robot_config, MagicMock())} module._world_monitor = None # Should not raise msg = JointState( - name=["left/joint1", "left/joint2", "left/joint3"], + name=["left_arm/joint1", "left_arm/joint2", "left_arm/joint3"], position=[0.1, 0.2, 0.3], ) module._on_joint_state(msg) @@ -659,15 +789,20 @@ def test_visualization_routing_and_stop_all_monitors(self): assert monitor.get_visualization_url() == "123" monitor.publish_visualization() - monitor.show_preview("robot") - monitor.hide_preview("robot") - path = _make_path([1.0], [2.0], [3.0]) - monitor.animate_path("robot", path, 4.5) + group_ids = ("robot/manipulator",) + plan = GeneratedPlan( + group_ids=group_ids, + path=[JointState(name=["robot/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) + monitor.show_preview(group_ids) + monitor.hide_preview(group_ids) + monitor.animate_plan(plan, 4.5) assert monitor.visualization is viz assert viz.published is True - assert viz.preview_shown == ["robot"] - assert viz.preview_hidden == ["robot"] - assert viz.animations == [("robot", path, 4.5)] + assert viz.preview_shown == [group_ids] + assert viz.preview_hidden == [group_ids] + assert viz.animations == [(plan, 4.5)] monitor.stop_all_monitors() @@ -680,9 +815,14 @@ def test_visualization_none_is_noop(self): assert monitor.get_visualization_url() is None monitor.publish_visualization() - monitor.show_preview("robot") - monitor.hide_preview("robot") - monitor.animate_path("robot", [1], 1.0) + plan = GeneratedPlan( + group_ids=("robot/manipulator",), + path=[JointState(name=["robot/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) + monitor.show_preview(("robot/manipulator",)) + monitor.hide_preview(("robot/manipulator",)) + monitor.animate_plan(plan, 1.0) monitor.start_visualization_thread() assert monitor._viz_thread is None @@ -691,90 +831,359 @@ class TestManipulationPreview: def test_dismiss_preview_noop_without_monitor(self): module = _make_module() - module._dismiss_preview("robot_id") + module._dismiss_preview(("arm/manipulator",)) def test_dismiss_preview_routes_to_monitor(self): module = _make_module() module._world_monitor = MagicMock() - module._dismiss_preview("robot_id") + group_ids = ("arm/manipulator",) + module._dismiss_preview(group_ids) - module._world_monitor.hide_preview.assert_called_once_with("robot_id") + module._world_monitor.hide_preview.assert_called_once_with(group_ids) module._world_monitor.publish_visualization.assert_called_once_with() - def test_preview_path_uses_trajectory_duration_and_interpolates(self): + def test_preview_plan_uses_safe_default_duration(self): module = _make_module() module._world_monitor = MagicMock() - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} - module._planned_paths = {"arm": _make_path([0.0], [2.0])} - module._planned_trajectories = {"arm": _make_trajectory((0.0, [0.0]), (2.0, [2.0]))} + module._last_plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) - assert module.preview_path(robot_name="arm", target_fps=2.0) is True + assert module.preview_plan() is True - module._world_monitor.animate_path.assert_called_once() - robot_id, preview_path, duration = module._world_monitor.animate_path.call_args.args - assert robot_id == "robot_id" - assert duration == 2.0 - assert [state.position for state in preview_path] == [[0.0], [0.5], [1.0], [1.5], [2.0]] + module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 1.0) - def test_preview_path_explicit_duration_overrides_and_fps_densifies(self): + def test_preview_plan_explicit_duration_overrides_default(self): module = _make_module() module._world_monitor = MagicMock() - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} - module._planned_paths = {"arm": _make_path([0.0], [9.0])} - module._planned_trajectories = {"arm": _make_trajectory((0.0, [0.0]), (9.0, [9.0]))} + module._last_plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) - assert module.preview_path(duration=1.5, robot_name="arm", target_fps=2.0) is True + assert module.preview_plan(duration=1.5) is True - module._world_monitor.animate_path.assert_called_once() - robot_id, preview_path, duration = module._world_monitor.animate_path.call_args.args - assert robot_id == "robot_id" - assert duration == 1.5 - assert [state.position for state in preview_path] == [[0.0], [3.0], [6.0], [9.0]] + module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 1.5) - def test_preview_path_missing_trajectory_uses_default_duration(self): + def test_preview_plan_respects_robot_filter(self): module = _make_module() module._world_monitor = MagicMock() - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} - module._planned_paths = {"arm": _make_path([0.0], [1.0])} - module._planned_trajectories = {} + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("arm", "manipulator", ["j1"])] + ) + module._last_plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) + + assert module.preview_plan(robot_name="arm") is True - assert module.preview_path(robot_name="arm", target_fps=10.0) is True + module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 1.0) - module._world_monitor.animate_path.assert_called_once_with( - "robot_id", module._planned_paths["arm"], 3.0 + def test_preview_plan_rejects_unaffected_robot_filter(self): + module = _make_module() + module._world_monitor = MagicMock() + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("arm", "manipulator", ["j1"])] ) + module._last_plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/j1"], position=[0.0])], + status=PlanningStatus.SUCCESS, + ) + + assert module.preview_plan(robot_name="other") is False + + module._world_monitor.animate_plan.assert_not_called() - def test_preview_path_skips_interpolation_for_nonpositive_fps_or_duration(self): + def test_preview_plan_returns_false_for_missing_inputs(self): module = _make_module() + + assert module.preview_plan() is False + module._world_monitor = MagicMock() - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} - module._planned_paths = {"arm": _make_path([0.0], [1.0])} - module._planned_trajectories = {"arm": _make_trajectory((0.0, [0.0]), (2.0, [1.0]))} + assert module.preview_plan() is False - assert module.preview_path(robot_name="arm", target_fps=0.0) is True - assert module.preview_path(duration=0.0, robot_name="arm", target_fps=20.0) is True - assert ( - module._world_monitor.animate_path.call_args_list[0].args[1] - == module._planned_paths["arm"] +class TestTargetSetEvaluation: + def test_evaluate_joint_target_set_projects_selected_targets_to_full_robot_states(self): + left_config = _make_robot_config("left", ["j1", "j2"], "left_task") + right_config = _make_robot_config("right", ["j1", "j2"], "right_task") + module = _make_module_with_monitor(left_config, right_config) + module._world_monitor.planning_groups = _FakePlanningGroups( + [ + _make_global_group("left", "arm", ["j1"]), + _make_global_group("right", "arm", ["j2"]), + ] ) - assert ( - module._world_monitor.animate_path.call_args_list[1].args[1] - == module._planned_paths["arm"] + module._world_monitor.get_current_joint_state.side_effect = [ + JointState(name=["j1", "j2"], position=[0.0, 9.0]), + JointState(name=["j1", "j2"], position=[8.0, 0.0]), + ] + module._world_monitor.is_state_valid.return_value = True + left_pose = PoseStamped(position=Vector3(x=1.0), orientation=Quaternion()) + right_pose = PoseStamped(position=Vector3(x=2.0), orientation=Quaternion()) + module._world_monitor.get_group_pose.side_effect = [left_pose, right_pose] + + result = module.evaluate_joint_target_set( + { + "left/arm": JointState(name=["j1"], position=[1.0]), + "right/arm": JointState(name=["j2"], position=[2.0]), + } ) - def test_preview_path_returns_false_for_missing_inputs(self): - module = _make_module() - module._planned_paths = {"arm": _make_path([0.0], [1.0])} - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} + assert result["success"] is True + assert result["collision_free"] is True + assert result["target_joints"] is not None + assert result["target_joints"].name == ["left/j1", "right/j2"] + assert result["target_joints"].position == [1.0, 2.0] + left_target = module._world_monitor.is_state_valid.call_args_list[0].args[1] + right_target = module._world_monitor.is_state_valid.call_args_list[1].args[1] + assert left_target.name == ["j1", "j2"] + assert left_target.position == [1.0, 9.0] + assert right_target.name == ["j1", "j2"] + assert right_target.position == [8.0, 2.0] + assert result["group_poses"] == {"left/arm": left_pose, "right/arm": right_pose} + + def test_evaluate_joint_target_set_reports_collision_per_group(self): + config = _make_robot_config("left", ["j1"], "task") + module = _make_module_with_monitor(config) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1"], position=[0.0] + ) + module._world_monitor.is_state_valid.return_value = False + module._world_monitor.get_group_pose.return_value = None - assert module.preview_path(robot_name="arm") is False + result = module.evaluate_joint_target_set( + {"left/manipulator": JointState(name=["j1"], position=[1.0])} + ) - module._world_monitor = MagicMock() - module._robots = {} - assert module.preview_path(robot_name="arm") is False + assert result["success"] is False + assert result["status"] == "COLLISION" + assert result["collision_free"] is False + assert result["message"] == "Target set is in collision" + assert result["group_diagnostics"] == {"left/manipulator": "Target is in collision"} + + def test_evaluate_pose_target_set_uses_whole_set_seed_and_auxiliary_groups(self): + config = _make_robot_config("left", ["j1", "j2"], "task") + arm_group = _make_global_group("left", "arm", ["j1"]) + wrist_group = _make_global_group("left", "wrist", ["j2"]) + module = _make_module_with_monitor(config) + module._world_monitor.planning_groups = _FakePlanningGroups([arm_group, wrist_group]) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1", "j2"], position=[0.1, 0.2] + ) + module._world_monitor.is_state_valid.return_value = True + module._world_monitor.get_group_pose.return_value = PoseStamped( + position=Vector3(x=1.0), orientation=Quaternion() + ) + module._kinematics = MagicMock() + module._kinematics.solve_pose_targets.return_value = IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState(name=["left/j1", "left/j2"], position=[1.0, 2.0]), + message="solved", + position_error=0.01, + orientation_error=0.02, + ) + pose = Pose(position=Vector3(x=0.5), orientation=Quaternion()) + + result = module.evaluate_pose_target_set( + {"left/arm": pose}, auxiliary_groups=("left/wrist",) + ) + + assert result["success"] is True + assert result["target_joints"] is not None + assert result["target_joints"].name == ["left/j1", "left/j2"] + assert result["target_joints"].position == [1.0, 2.0] + _, kwargs = module._kinematics.solve_pose_targets.call_args + assert list(kwargs["pose_targets"]) == [arm_group] + assert kwargs["auxiliary_groups"] == (wrist_group,) + assert kwargs["seed"].name == ["left/j1", "left/j2"] + assert kwargs["seed"].position == [0.1, 0.2] + + def test_evaluate_pose_target_set_can_skip_collision_checking(self): + config = _make_robot_config("left", ["j1"], "task") + group = _make_global_group("left", "arm", ["j1"]) + module = _make_module_with_monitor(config) + module._world_monitor.planning_groups = _FakePlanningGroups([group]) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1"], position=[0.0] + ) + module._world_monitor.is_state_valid.return_value = False + module._world_monitor.get_group_pose.return_value = PoseStamped( + position=Vector3(x=1.0), orientation=Quaternion() + ) + module._kinematics = MagicMock() + module._kinematics.solve_pose_targets.return_value = IKResult( + status=IKStatus.SUCCESS, + joint_state=JointState(name=["left/j1"], position=[1.0]), + message="solved", + ) + + result = module.evaluate_pose_target_set( + {"left/arm": Pose(position=Vector3(x=0.5), orientation=Quaternion())}, + check_collision=False, + ) - module._robots = {"arm": ("robot_id", MagicMock(), MagicMock())} - module._planned_paths = {"arm": []} - assert module.preview_path(robot_name="arm") is False + assert result["success"] is True + assert result["collision_free"] is True + assert result["group_diagnostics"] == {"left/arm": "Target collision check skipped"} + module._world_monitor.is_state_valid.assert_not_called() + + +class TestGeneratedPlanProjection: + def test_selected_joint_state_accepts_local_current_state_names(self): + config = _make_robot_config("left", ["j1", "j2"], "task") + module = _make_module_with_monitor(config) + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("left", "arm", ["j1", "j2"])] + ) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1", "j2"], position=[1.0, 2.0] + ) + + selected = module._selected_joint_state(("left/arm",)) + + assert selected is not None + assert selected.name == ["left/j1", "left/j2"] + assert selected.position == [1.0, 2.0] + + def test_selected_joint_state_rejects_mixed_current_state_names(self): + config = _make_robot_config("left", ["j1", "j2"], "task") + module = _make_module_with_monitor(config) + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("left", "arm", ["j1", "j2"])] + ) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["left/j1", "j2"], position=[1.0, 2.0] + ) + + assert module._selected_joint_state(("left/arm",)) is None + + def test_execute_plan_dispatches_one_trajectory_per_affected_robot(self): + left_config = _make_robot_config( + "left", + ["j1", "j2", "j3"], + "left_task", + ) + right_config = _make_robot_config("right", ["j1", "j2"], "right_task") + module = _make_module_with_monitor(left_config, right_config) + left_gen = _trajectory_generator() + right_gen = _trajectory_generator() + module._robots["left"] = ("robot_left", left_config, left_gen) + module._robots["right"] = ("robot_right", right_config, right_gen) + module._world_monitor.planning_groups = _FakePlanningGroups( + [ + _make_global_group("left", "arm", ["j1", "j2"]), + _make_global_group("right", "arm", ["j1"]), + ] + ) + module._world_monitor.get_current_joint_state.side_effect = [ + JointState(name=["j1", "j2", "j3"], position=[0.0, 0.0, 9.0]), + JointState(name=["j1", "j2"], position=[0.0, 8.0]), + ] + module._coordinator_client = MagicMock() + module._coordinator_client.task_invoke.return_value = True + plan = _make_generated_plan(("left/arm", "right/arm"), [1.0, 2.0, 3.0], [4.0, 5.0, 6.0]) + + assert module.execute_plan(plan) is True + + assert module._coordinator_client.task_invoke.call_count == 2 + left_call, right_call = module._coordinator_client.task_invoke.call_args_list + assert left_call.args[0:2] == ("left_task", "execute") + left_trajectory = left_call.args[2]["trajectory"] + assert left_trajectory.joint_names == ["left/j1", "left/j2", "left/j3"] + assert [point.positions for point in left_trajectory.points] == [ + [1.0, 2.0, 9.0], + [4.0, 5.0, 9.0], + ] + assert right_call.args[0:2] == ("right_task", "execute") + right_trajectory = right_call.args[2]["trajectory"] + assert right_trajectory.joint_names == ["right/j1", "right/j2"] + assert [point.positions for point in right_trajectory.points] == [[3.0, 8.0], [6.0, 8.0]] + + def test_execute_plan_holds_non_selected_joints_from_current_state(self): + config = _make_robot_config("left", ["j1", "j2", "j3"], "task") + module = _make_module_with_monitor(config) + generator = _trajectory_generator() + module._robots["left"] = ("robot_left", config, generator) + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("left", "arm", ["j2"])] + ) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1", "j2", "j3"], position=[10.0, 20.0, 30.0] + ) + module._coordinator_client = MagicMock() + module._coordinator_client.task_invoke.return_value = True + plan = GeneratedPlan( + group_ids=("left/arm",), + path=[ + JointState(name=["left/j2"], position=[2.0]), + JointState(name=["left/j2"], position=[3.0]), + ], + status=PlanningStatus.SUCCESS, + ) + + assert module.execute_plan(plan) is True + + trajectory = module._coordinator_client.task_invoke.call_args.args[2]["trajectory"] + assert trajectory.joint_names == ["left/j1", "left/j2", "left/j3"] + assert [point.positions for point in trajectory.points] == [ + [10.0, 2.0, 30.0], + [10.0, 3.0, 30.0], + ] + + def test_execute_plan_rejects_local_waypoint_names(self): + config = _make_robot_config("left", ["j1", "j2"], "task") + module = _make_module_with_monitor(config) + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("left", "arm", ["j1"])] + ) + module._world_monitor.get_current_joint_state.return_value = JointState( + name=["j1", "j2"], position=[10.0, 20.0] + ) + module._coordinator_client = MagicMock() + plan = GeneratedPlan( + group_ids=("left/arm",), + path=[JointState(name=["j1"], position=[1.0])], + status=PlanningStatus.SUCCESS, + ) + + assert module.execute_plan(plan) is False + module._coordinator_client.task_invoke.assert_not_called() + + def test_preview_plan_with_last_plan_animates_generated_plan(self): + config = _make_robot_config("left", ["j1", "j2"], "task") + module = _make_module_with_monitor(config) + module._world_monitor.planning_groups = _FakePlanningGroups( + [_make_global_group("left", "arm", ["j1"])] + ) + module._last_plan = GeneratedPlan( + group_ids=("left/arm",), + path=[ + JointState(name=["left/j1"], position=[1.0]), + JointState(name=["left/j1"], position=[2.0]), + ], + status=PlanningStatus.SUCCESS, + ) + + assert module.preview_plan(robot_name="left") is True + + module._world_monitor.animate_plan.assert_called_once_with(module._last_plan, 1.0) + + def test_has_and_clear_planned_path_use_last_plan(self): + module = _make_module() + module._last_plan = GeneratedPlan( + group_ids=("left/arm",), + path=[JointState(name=["left/j1"], position=[1.0])], + status=PlanningStatus.SUCCESS, + ) + assert module.has_planned_path() is True + assert module.clear_planned_path() is True + assert module.has_planned_path() is False + assert module._last_plan is None diff --git a/dimos/manipulation/visualization/test_factory.py b/dimos/manipulation/visualization/test_factory.py index 4d49f019cf..3f9e22d9d9 100644 --- a/dimos/manipulation/visualization/test_factory.py +++ b/dimos/manipulation/visualization/test_factory.py @@ -14,6 +14,7 @@ from __future__ import annotations +from collections.abc import Sequence from contextlib import AbstractContextManager, nullcontext from pathlib import Path from unittest.mock import MagicMock @@ -26,8 +27,9 @@ from dimos.manipulation.manipulation_module import ManipulationModuleConfig from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( - JointPath, + GeneratedPlan, Obstacle, + PlanningGroupID, PlanningSceneInfo, WorldRobotID, ) @@ -52,13 +54,13 @@ def get_visualization_url(self) -> str | None: def publish_visualization(self, ctx: object | None = None) -> None: return None - def show_preview(self, robot_id: WorldRobotID) -> None: + def show_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: return None - def hide_preview(self, robot_id: WorldRobotID) -> None: + def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: return None - def animate_path(self, robot_id: WorldRobotID, path: JointPath, duration: float = 3.0) -> None: + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: return None def close(self) -> None: diff --git a/dimos/manipulation/visualization/types.py b/dimos/manipulation/visualization/types.py index 778c293499..a4c22edcd7 100644 --- a/dimos/manipulation/visualization/types.py +++ b/dimos/manipulation/visualization/types.py @@ -16,7 +16,7 @@ from typing import TypedDict -from dimos.manipulation.planning.spec.models import RobotName, WorldRobotID +from dimos.manipulation.planning.spec.models import PlanningGroupID, RobotName, WorldRobotID from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState @@ -33,7 +33,20 @@ class TargetEvaluation(TypedDict, total=False): orientation_error: float -class RobotInfo(TypedDict): +class TargetSetEvaluation(TypedDict, total=False): + success: bool + status: str + message: str + collision_free: bool + group_ids: tuple[PlanningGroupID, ...] + target_joints: JointState | None + group_diagnostics: dict[PlanningGroupID, str] + group_poses: dict[PlanningGroupID, PoseStamped | Pose | None] + position_error: float + orientation_error: float + + +class RobotInfo(TypedDict, total=False): name: RobotName world_robot_id: WorldRobotID joint_names: list[str] @@ -46,3 +59,16 @@ class RobotInfo(TypedDict): home_joints: list[float] | None pre_grasp_offset: float init_joints: list[float] | None + planning_groups: list[PlanningGroupInfo] + + +class PlanningGroupInfo(TypedDict): + id: PlanningGroupID + name: str + robot_name: RobotName + joint_names: list[str] + local_joint_names: list[str] + base_link: str + tip_link: str | None + has_pose_target: bool + source: str diff --git a/dimos/manipulation/visualization/viser/adapter.py b/dimos/manipulation/visualization/viser/adapter.py index c3eb8e360b..82acc1f63a 100644 --- a/dimos/manipulation/visualization/viser/adapter.py +++ b/dimos/manipulation/visualization/viser/adapter.py @@ -15,16 +15,22 @@ from __future__ import annotations from collections.abc import Sequence -from typing import TYPE_CHECKING - -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from typing import TYPE_CHECKING, cast + +from dimos.manipulation.visualization.types import ( + PlanningGroupInfo, + RobotInfo, + TargetEvaluation, + TargetSetEvaluation, +) from dimos.msgs.sensor_msgs.JointState import JointState if TYPE_CHECKING: from dimos.manipulation.manipulation_module import ManipulationModule + from dimos.manipulation.planning.groups.models import PlanningGroup from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor from dimos.manipulation.planning.spec.config import RobotModelConfig - from dimos.manipulation.planning.spec.models import JointPath, RobotName, WorldRobotID + from dimos.manipulation.planning.spec.models import PlanningGroupID, RobotName, WorldRobotID from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -73,7 +79,7 @@ def get_robot_info(self, robot_name: RobotName) -> RobotInfo | None: "base_link": str(info["base_link"]), "max_velocity": float(info["max_velocity"]), "max_acceleration": float(info["max_acceleration"]), - "has_joint_name_mapping": bool(info["has_joint_name_mapping"]), + "has_joint_name_mapping": bool(info.get("has_joint_name_mapping", False)), "coordinator_task_name": None if info["coordinator_task_name"] is None else str(info["coordinator_task_name"]), @@ -84,8 +90,31 @@ def get_robot_info(self, robot_name: RobotName) -> RobotInfo | None: "init_joints": None if info["init_joints"] is None else [float(value) for value in info["init_joints"]], + "planning_groups": [ + { + "id": str(group["id"]), + "name": str(group["name"]), + "robot_name": str(info["name"]), + "joint_names": [str(name) for name in group["joint_names"]], + "local_joint_names": [str(name) for name in group["local_joint_names"]], + "base_link": str(group["base_link"]), + "tip_link": None if group["tip_link"] is None else str(group["tip_link"]), + "has_pose_target": bool(group["has_pose_target"]), + "source": str(group["source"]), + } + for group in info.get("planning_groups", []) + ], } + def list_planning_groups(self) -> list[PlanningGroupInfo]: + groups: list[PlanningGroupInfo] = [] + for robot_name in self.list_robots(): + info = self.get_robot_info(robot_name) + if info is None: + continue + groups.extend(info.get("planning_groups", [])) + return groups + def get_init_joints(self, robot_name: RobotName) -> JointState | None: return copy_joint_state(self._module.get_init_joints(robot_name)) @@ -118,24 +147,59 @@ def evaluate_joint_target(self, joints: JointState, robot_name: RobotName) -> Ta ) return result - def evaluate_pose_target(self, pose: Pose, robot_name: RobotName) -> TargetEvaluation: + def evaluate_pose_target( + self, pose: Pose, robot_name: RobotName, *, check_collision: bool = True + ) -> TargetEvaluation: """Evaluate a Cartesian target through module/WorldMonitor helper boundaries.""" - result: TargetEvaluation = {**self._module.evaluate_pose_target(pose, robot_name)} + result: TargetEvaluation = { + **self._module.evaluate_pose_target(pose, robot_name, check_collision=check_collision) + } joint_state = result.get("joint_state") result["joint_state"] = copy_joint_state( joint_state if isinstance(joint_state, JointState) else None ) return result - def get_planned_path(self, robot_name: RobotName) -> JointPath | None: - path = self._module.get_planned_path(robot_name) - if path is None: - return None - copied = [copy_joint_state(point) for point in path] - return [point for point in copied if point is not None] + def evaluate_joint_target_set( + self, joint_targets: dict[PlanningGroupID, JointState] + ) -> TargetSetEvaluation: + result: TargetSetEvaluation = { + **self._module.evaluate_joint_target_set( + cast( + "dict[PlanningGroupID | PlanningGroup, JointState]", + { + group_id: copy_joint_state(target) or target + for group_id, target in joint_targets.items() + }, + ) + ) + } + target_joints = result.get("target_joints") + result["target_joints"] = copy_joint_state( + target_joints if isinstance(target_joints, JointState) else None + ) + return result - def get_planned_trajectory_duration(self, robot_name: RobotName) -> float | None: - return self._module.get_planned_trajectory_duration(robot_name) + def evaluate_pose_target_set( + self, + pose_targets: dict[PlanningGroupID, Pose | PoseStamped], + auxiliary_groups: Sequence[PlanningGroupID] = (), + seed: JointState | None = None, + check_collision: bool = True, + ) -> TargetSetEvaluation: + result: TargetSetEvaluation = { + **self._module.evaluate_pose_target_set( + cast("dict[PlanningGroupID | PlanningGroup, Pose | PoseStamped]", pose_targets), + auxiliary_groups=auxiliary_groups, + seed=copy_joint_state(seed), + check_collision=check_collision, + ) + } + target_joints = result.get("target_joints") + result["target_joints"] = copy_joint_state( + target_joints if isinstance(target_joints, JointState) else None + ) + return result def get_module_state(self) -> str: return str(self._module.get_state()) @@ -152,11 +216,16 @@ def plan_to_pose(self, pose: Pose, robot_name: RobotName | None = None) -> bool: def plan_to_joints(self, joints: JointState, robot_name: RobotName | None = None) -> bool: return self._module.plan_to_joints(joints, robot_name) - def preview_path(self, robot_name: RobotName | None = None) -> bool: - return self._module.preview_path(robot_name=robot_name) + def plan_target_set(self, joint_targets: dict[PlanningGroupID, JointState]) -> bool: + return self._module.plan_to_joint_targets( + cast("dict[PlanningGroupID | PlanningGroup, JointState]", joint_targets) + ) + + def preview_plan(self, robot_name: RobotName | None = None) -> bool: + return self._module.preview_plan(robot_name=robot_name) - def execute(self, robot_name: RobotName | None = None) -> bool: - return self._module.execute(robot_name) + def execute(self) -> bool: + return self._module.execute() def cancel(self) -> bool: return self._module.cancel() diff --git a/dimos/manipulation/visualization/viser/animation.py b/dimos/manipulation/visualization/viser/animation.py index f5a574d82c..03b4e3321e 100644 --- a/dimos/manipulation/visualization/viser/animation.py +++ b/dimos/manipulation/visualization/viser/animation.py @@ -15,11 +15,31 @@ from __future__ import annotations from collections.abc import Callable, Sequence +from dataclasses import dataclass import time +from dimos.manipulation.planning.spec.models import PlanningGroupID from dimos.msgs.sensor_msgs.JointState import JointState +@dataclass(frozen=True) +class PreviewTrack: + """One render track owned by one or more planning groups.""" + + robot_id: str + group_ids: tuple[PlanningGroupID, ...] + joint_names: tuple[str, ...] + path: tuple[JointState, ...] + + +@dataclass(frozen=True) +class GroupPreviewAnimation: + """Group-native preview transaction for a generated plan.""" + + group_ids: tuple[PlanningGroupID, ...] + tracks: tuple[PreviewTrack, ...] + + def interpolate_joint_path( path: Sequence[JointState], duration: float, fps: float ) -> list[list[float]]: @@ -57,10 +77,10 @@ def sampled_joint_path_frames( ) -> list[list[float]]: """Return animation frames while preserving already sampled trajectories. - ManipulationModule.preview_path() owns trajectory-aware interpolation because it has access - to JointTrajectory waypoint timing. If a path arrives already sampled near the target display - rate, Viser should play those samples directly instead of re-interpolating by waypoint index. - Sparse direct VisualizationSpec callers still get local interpolation as a fallback. + ViserManipulationVisualizer.animate_plan() projects GeneratedPlan waypoints into robot-local + preview paths. If a path arrives already sampled near the target display rate, Viser should + play those samples directly instead of re-interpolating by waypoint index. Sparse direct scene + callers still get local interpolation as a fallback. """ waypoints = [list(waypoint.position) for waypoint in path if waypoint.position] if not waypoints: @@ -92,7 +112,8 @@ def animate(self, path: Sequence[JointState], duration: float, fps: float) -> bo if not frames: return False step_delay = duration / max(len(frames) - 1, 1) if duration > 0.0 else 0.0 - for joints in frames: + for index, joints in enumerate(frames): self._set_joints(joints) - self._sleep(step_delay) + if index < len(frames) - 1: + self._sleep(step_delay) return True diff --git a/dimos/manipulation/visualization/viser/gui.py b/dimos/manipulation/visualization/viser/gui.py index 0434228583..5bb7a00d0c 100644 --- a/dimos/manipulation/visualization/viser/gui.py +++ b/dimos/manipulation/visualization/viser/gui.py @@ -16,7 +16,12 @@ from typing import TypeAlias -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from dimos.manipulation.planning.spec.models import PlanningGroupID +from dimos.manipulation.visualization.types import ( + PlanningGroupInfo, + TargetEvaluation, + TargetSetEvaluation, +) from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.runtime import VISER_INSTALL_HINT @@ -68,6 +73,9 @@ # Fallback joint-slider range (radians) when a robot config omits joint limits. DEFAULT_JOINT_LIMITS = (-3.14, 3.14) +PRIMARY_ACTION_COLOR = (0, 102, 179) +ACTIVE_GROUP_COLOR = PRIMARY_ACTION_COLOR +INACTIVE_GROUP_COLOR = (52, 52, 52) class ViserPanelGui: @@ -88,6 +96,7 @@ def __init__( self._closed = False self._operation_sequence_id = 0 self._suppress_target_callbacks = False + self._default_group_initialized = False self._handles: dict[str, PanelHandle] = {} self._joint_sliders: dict[str, GuiSliderHandle[float]] = {} self._worker = TargetEvaluationWorker( @@ -128,18 +137,27 @@ def refresh(self) -> None: if self._closed: return robots = self.adapter.list_robots() + groups = self.adapter.list_planning_groups() self.state.backend_status = ( BackendConnectionStatus.READY if robots else BackendConnectionStatus.WAITING_FOR_ROBOT ) - if self.state.selected_robot is None and robots: - self.state.selected_robot = robots[0] + if not self.state.selected_group_ids and groups and not self._default_group_initialized: + first_pose_group = next( + (group for group in groups if bool(group["has_pose_target"])), groups[0] + ) + self.state.selected_group_ids = (str(first_pose_group["id"]),) self.state.target_status = TargetStatus.EMPTY + self._default_group_initialized = True + self._sync_group_selection_state() + self._initialize_selected_group_targets() self._build_joint_sliders() - self._sync_robot_dropdown(robots) + self._sync_group_selector(groups) self._refresh_selected_robot_state() self._ensure_scene_controls() + self._sync_target_ghost_visibility() self._sync_preset_dropdown() self._update_status_text() + self._update_target_summary() self._update_control_state() def _build(self) -> None: @@ -150,26 +168,28 @@ def _build(self) -> None: self._build_panel_controls(gui) def _build_panel_controls(self, gui: GuiApi) -> None: - self._handles["status"] = gui.add_markdown("Starting manipulation panel...") - robots = self.adapter.list_robots() + self._handles["status"] = gui.add_markdown("### Status\n**State:** Ready") self._build_scene_controls(gui) - robot_dropdown = gui.add_dropdown( - "Robot", - options=robots or [""], - initial_value=robots[0] if robots else "", + self._handles["planning_groups_heading"] = gui.add_markdown( + "### Planning Groups\nActive MoveIt group for pose goal, planning, and joint edits." ) - robot_dropdown.on_update(lambda event: self._select_robot(event.target.value)) - self._handles["robot"] = robot_dropdown + self._sync_group_selector(self.adapter.list_planning_groups()) + self._handles["target_heading"] = gui.add_markdown("### Target") preset_dropdown = gui.add_dropdown( - "Target Preset", + "Preset", options=["Select preset...", "Current"], initial_value="Select preset...", ) preset_dropdown.on_update(lambda event: self._apply_preset(event.target.value)) self._handles["preset"] = preset_dropdown - plan_button = gui.add_button("Plan", disabled=True) + self._handles["target_summary"] = gui.add_markdown( + f"Feasibility: `{self.state.feasibility.status.value}`" + ) + self._handles["actions_heading"] = gui.add_markdown("### Actions") + plan_button = gui.add_button("Plan", disabled=True, color=PRIMARY_ACTION_COLOR) plan_button.on_click(lambda _: self._submit_plan()) self._handles["plan"] = plan_button + self._handles["plan_controls_heading"] = gui.add_markdown("**Plan controls**") preview_button = gui.add_button("Preview", disabled=True) preview_button.on_click(lambda _: self._submit_preview()) self._handles["preview"] = preview_button @@ -182,6 +202,8 @@ def _build_panel_controls(self, gui: GuiApi) -> None: clear_button = gui.add_button("Clear plan") clear_button.on_click(lambda _: self._submit_clear()) self._handles["clear"] = clear_button + joint_controls = gui.add_folder("Joint Control", expand_by_default=False) + self._handles["joint_control_folder"] = joint_controls self._build_joint_sliders() def _build_scene_controls(self, gui: GuiApi) -> None: @@ -218,56 +240,133 @@ def _refresh_selected_robot_state(self) -> None: self.state.error = adapter_error def _ensure_scene_controls(self) -> None: - if self.scene is None or self.state.selected_robot is None: - return - robot_id = self.adapter.robot_id_for_name(self.state.selected_robot) - if robot_id is None: + if self.scene is None: return - ee_control = self.scene.ensure_target_controls(str(robot_id), self._on_transform_update) - if ee_control is not None: - self._handles["ee_control"] = ee_control - if ( - self.state.target_status == TargetStatus.EMPTY - and self.state.current_ee_pose is not None - ): - self.state.cartesian_target = self.state.current_ee_pose - self._suppress_target_callbacks = True - try: - self.scene.set_target_pose(str(robot_id), self.state.current_ee_pose) - finally: - self._suppress_target_callbacks = False + groups = self._group_info_by_id() + active_pose_groups = set(self._selected_pose_group_ids()) + for key in [key for key in self._handles if key.startswith("ee_control:")]: + group_id = key.split(":", 1)[1] + if group_id in active_pose_groups: + continue + handle = self._handles.pop(key) + remove_target_controls = getattr(self.scene, "remove_target_controls", None) + if callable(remove_target_controls): + remove_target_controls(group_id) + else: + remove = getattr(handle, "remove", None) + if callable(remove): + remove() + for group_id in active_pose_groups: + group = groups.get(group_id) + if group is None or not bool(group["has_pose_target"]): + continue + handle_key = f"ee_control:{group_id}" + if handle_key in self._handles: + continue + ee_control = self.scene.ensure_target_controls( + group_id, + lambda target, gid=group_id: self._on_transform_update(gid, target), + ) + if ee_control is not None: + self._handles[handle_key] = ee_control + pose = self.state.pose_targets.get(group_id) + if pose is not None: + self._suppress_target_callbacks = True + try: + self.scene.set_target_pose(group_id, pose) + finally: + self._suppress_target_callbacks = False def _build_joint_sliders(self) -> None: - if self.state.selected_robot is None: - return gui = self.server.gui - config = self.adapter.get_robot_config(self.state.selected_robot) - if config is None: - return - current = self.adapter.get_current_joint_state(self.state.selected_robot) - values = list(current.position) if current is not None else [0.0] * len(config.joint_names) self._clear_joint_sliders() - joint_limits_lower = config.joint_limits_lower - joint_limits_upper = config.joint_limits_upper - for index, joint_name in enumerate(config.joint_names): - lower, upper = DEFAULT_JOINT_LIMITS - if joint_limits_lower is not None and index < len(joint_limits_lower): - lower = joint_limits_lower[index] - if joint_limits_upper is not None and index < len(joint_limits_upper): - upper = joint_limits_upper[index] - handle = gui.add_slider( - joint_name, - min=float(lower), - max=float(upper), - step=0.001, - initial_value=float(values[index] if index < len(values) else 0.0), + if not self.state.selected_group_ids: + return + joint_folder = self._handles.get("joint_control_folder") + if joint_folder is not None: + with joint_folder: + self._build_joint_slider_handles(gui) + return + self._build_joint_slider_handles(gui) + + def _build_joint_slider_handles(self, gui: GuiApi) -> None: + groups = self._group_info_by_id() + target_by_name: dict[str, float] = {} + if self.state.target_joints is not None: + target_by_name.update( + zip(self.state.target_joints.name, self.state.target_joints.position, strict=False) ) + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + config = self.adapter.get_robot_config(str(group["robot_name"])) + current = self.adapter.get_current_joint_state(str(group["robot_name"])) + current_by_name = ( + dict(zip(current.name, current.position, strict=False)) + if current is not None + else {} + ) + joint_limits_lower = config.joint_limits_lower if config is not None else None + joint_limits_upper = config.joint_limits_upper if config is not None else None + for index, (global_name, local_name) in enumerate( + zip(group["joint_names"], group["local_joint_names"], strict=False) + ): + joint_name = str(global_name) + local = str(local_name) + value = float(target_by_name.get(joint_name, current_by_name.get(local, 0.0))) + lower, upper = DEFAULT_JOINT_LIMITS + if joint_limits_lower is not None and index < len(joint_limits_lower): + lower = joint_limits_lower[index] + if joint_limits_upper is not None and index < len(joint_limits_upper): + upper = joint_limits_upper[index] + handle = gui.add_slider( + f"{group_id}/{local}", + min=float(lower), + max=float(upper), + step=0.001, + initial_value=value, + ) - def on_update(_event: object, name: str = joint_name) -> None: - self._on_joint_slider_update(name) - - handle.on_update(on_update) - self._joint_sliders[joint_name] = handle + def on_update(_event: object, name: str = joint_name) -> None: + self._on_joint_slider_update(name) + + handle.on_update(on_update) + self._joint_sliders[joint_name] = handle + + def _target_set_from_sliders(self) -> dict[PlanningGroupID, JointState] | None: + groups = self._group_info_by_id() + targets: dict[PlanningGroupID, JointState] = {} + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + self._set_error(f"Unknown planning group: {group_id}") + return None + names = [str(name) for name in group["joint_names"]] + positions: list[float] = [] + for name in names: + handle = self._joint_sliders.get(name) + if handle is None: + self._set_error(f"Missing target slider for {name}") + return None + positions.append(float(handle.value)) + targets[group_id] = JointState({"name": names, "position": positions}) + return targets + + def _split_target_joints_by_group(self, target_joints: JointState) -> None: + groups = self._group_info_by_id() + positions_by_name = dict(zip(target_joints.name, target_joints.position, strict=False)) + self.state.group_joint_targets.clear() + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + names = [str(name) for name in group["joint_names"]] + if not all(name in positions_by_name for name in names): + continue + self.state.group_joint_targets[group_id] = JointState( + {"name": names, "position": [float(positions_by_name[name]) for name in names]} + ) def _clear_joint_sliders(self) -> None: for handle in self._joint_sliders.values(): @@ -284,51 +383,219 @@ def _remove_panel_handles(self) -> None: remove() self._handles.pop(key, None) - def _select_robot(self, robot_name: str) -> None: + def _sync_group_selector(self, groups: list[PlanningGroupInfo]) -> None: + seen_keys: set[str] = set() + selected = set(self.state.selected_group_ids) + for group in sorted( + groups, key=lambda item: (not bool(item["has_pose_target"]), str(item["id"])) + ): + group_id = str(group["id"]) + key = f"group:{group_id}" + seen_keys.add(key) + handle = self._handles.get(key) + is_selected = group_id in selected + label = self._group_selector_label(group, selected=is_selected) + if handle is None: + handle = self.server.gui.add_button( + label, + color=self._group_selector_color(is_selected), + hint="Click to toggle this planning group in the target set.", + ) + handle.on_click(lambda _event, gid=group_id: self._toggle_group_selected(gid)) + self._handles[key] = handle + else: + self._set_optional_handle_attr(handle, "label", label) + self._set_optional_handle_attr( + handle, "color", self._group_selector_color(is_selected) + ) + + for key in [key for key in self._handles if key.startswith("group:")]: + if key not in seen_keys: + handle = self._handles.pop(key) + remove = getattr(handle, "remove", None) + if callable(remove): + remove() + + @staticmethod + def _group_selector_label(group: PlanningGroupInfo, *, selected: bool = False) -> str: + _ = selected + return ViserPanelGui._group_display_name(group) + + @staticmethod + def _group_selector_color(selected: bool) -> tuple[int, int, int] | None: + return ACTIVE_GROUP_COLOR if selected else INACTIVE_GROUP_COLOR + + @staticmethod + def _group_display_name(group: PlanningGroupInfo) -> str: + robot_name = str(group["robot_name"]) + group_name = str(group["name"]) + return robot_name if group_name == "manipulator" else f"{robot_name} {group_name}" + + def _set_group_selected(self, group_id: PlanningGroupID, selected: bool) -> None: + current = list(self.state.selected_group_ids) + if selected and group_id not in current: + current.append(group_id) + elif not selected and group_id in current: + current.remove(group_id) + self.state.selected_group_ids = tuple(current) + self._sync_group_selection_state() + self._prune_inactive_group_state() + self._initialize_selected_group_targets() + self.state.mark_plan_stale() + self._build_joint_sliders() + self.refresh() + + def _toggle_group_selected(self, group_id: PlanningGroupID) -> None: + self._set_group_selected(group_id, group_id not in self.state.selected_group_ids) + + def _select_all_manipulators(self) -> None: + groups = self.adapter.list_planning_groups() + manipulator_groups = [ + str(group["id"]) for group in groups if str(group["name"]) == "manipulator" + ] + self.state.selected_group_ids = tuple( + manipulator_groups or [str(group["id"]) for group in groups] + ) + self._sync_group_selection_state() + self._initialize_selected_group_targets() + self._build_joint_sliders() + self.refresh() + + def _clear_group_selection(self) -> None: if self._closed: return - if (robot_name or None) == self.state.selected_robot: - self.refresh() - return - self.state.selected_robot = robot_name or None + self.state.selected_group_ids = () + self._sync_group_selection_state() + self._prune_inactive_group_state() self.state.target_status = TargetStatus.EMPTY self.state.feasibility.status = FeasibilityStatus.UNKNOWN self.state.plan_state = PanelPlanState() self._build_joint_sliders() - self._sync_preset_dropdown() self.refresh() - def _sync_robot_dropdown(self, robots: list[str]) -> None: - handle = self._handles.get("robot") - if handle is None: - return - options = robots or [""] - for attr in ("options", "values"): - if hasattr(handle, attr): - try: - self._set_optional_handle_attr(handle, attr, options) - except Exception: - logger.warning("Could not set robot dropdown %s", attr, exc_info=True) - if hasattr(handle, "value") and self.state.selected_robot in robots: - try: - self._set_optional_handle_attr(handle, "value", self.state.selected_robot) - except Exception: - logger.warning("Could not set robot dropdown value", exc_info=True) + def _group_info_by_id(self) -> dict[PlanningGroupID, PlanningGroupInfo]: + return {str(group["id"]): group for group in self.adapter.list_planning_groups()} + + def _sync_selected_robot_from_groups(self) -> None: + groups = self._group_info_by_id() + first_group = ( + groups.get(self.state.selected_group_ids[0]) if self.state.selected_group_ids else None + ) + self.state.selected_robot = None if first_group is None else str(first_group["robot_name"]) + + def _sync_group_selection_state(self) -> None: + self._sync_selected_robot_from_groups() + self.state.auxiliary_group_ids = self._selected_auxiliary_group_ids() + + def _selected_pose_group_ids(self) -> tuple[PlanningGroupID, ...]: + groups = self._group_info_by_id() + return tuple( + group_id + for group_id in self.state.selected_group_ids + if (group := groups.get(group_id)) is not None and bool(group["has_pose_target"]) + ) + + def _selected_auxiliary_group_ids(self) -> tuple[PlanningGroupID, ...]: + groups = self._group_info_by_id() + return tuple( + group_id + for group_id in self.state.selected_group_ids + if (group := groups.get(group_id)) is not None and not bool(group["has_pose_target"]) + ) + + def _active_pose_targets(self) -> dict[PlanningGroupID, Pose]: + return { + group_id: self.state.pose_targets[group_id] + for group_id in self._selected_pose_group_ids() + if group_id in self.state.pose_targets + } + + def _prune_inactive_group_state(self) -> None: + selected = set(self.state.selected_group_ids) + for mapping in ( + self.state.pose_targets, + self.state.group_joint_targets, + self.state.group_poses, + self.state.group_diagnostics, + ): + for group_id in [group_id for group_id in mapping if group_id not in selected]: + mapping.pop(group_id, None) + self._refresh_target_joints_from_groups() + + def _initialize_selected_group_targets(self) -> None: + groups = self._group_info_by_id() + for group_id in self.state.selected_group_ids: + if group_id in self.state.group_joint_targets: + continue + group = groups.get(group_id) + if group is None: + continue + current = self.adapter.get_current_joint_state(str(group["robot_name"])) + if current is None: + continue + current_by_name = dict(zip(current.name, current.position, strict=False)) + names = [str(name) for name in group["joint_names"]] + local_names = [str(name) for name in group["local_joint_names"]] + positions = [float(current_by_name.get(local, 0.0)) for local in local_names] + self.state.group_joint_targets[group_id] = JointState( + {"name": names, "position": positions} + ) + if bool(group["has_pose_target"]) and group_id not in self.state.pose_targets: + pose = self.adapter.get_ee_pose(str(group["robot_name"])) + if pose is not None: + self.state.pose_targets[group_id] = pose + self.state.group_poses[group_id] = pose + if self.state.cartesian_target is None: + self.state.cartesian_target = pose + self._refresh_target_joints_from_groups() + + def _refresh_target_joints_from_groups(self) -> None: + names: list[str] = [] + positions: list[float] = [] + for group_id in self.state.selected_group_ids: + target = self.state.group_joint_targets.get(group_id) + if target is None: + continue + names.extend(target.name) + positions.extend(target.position) + self.state.target_joints = ( + JointState({"name": names, "position": positions}) if names else None + ) + + def _current_snapshot_by_group(self) -> dict[PlanningGroupID, list[float]]: + groups = self._group_info_by_id() + snapshot: dict[PlanningGroupID, list[float]] = {} + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + current = self.adapter.get_current_joint_state(str(group["robot_name"])) + if current is None: + continue + current_by_name = dict(zip(current.name, current.position, strict=False)) + snapshot[group_id] = [ + float(current_by_name.get(str(local_name), 0.0)) + for local_name in group["local_joint_names"] + ] + return snapshot def _sync_preset_dropdown(self) -> None: handle = self._handles.get("preset") - if handle is None or self.state.selected_robot is None: + if handle is None: return - info: RobotInfo | None = self.adapter.get_robot_info(self.state.selected_robot) - config = self.adapter.get_robot_config(self.state.selected_robot) + selected_robot_names = self._selected_robot_names() options = ["Select preset..."] - if (info is not None and info["init_joints"] is not None) or self.adapter.get_init_joints( - self.state.selected_robot - ) is not None: + if any( + self.adapter.get_init_joints(robot_name) is not None + for robot_name in selected_robot_names + ): options.append("Init") options.append("Current") - home_joints = config.home_joints if config is not None else None - if (info is not None and info["home_joints"] is not None) or home_joints is not None: + if any( + (config := self.adapter.get_robot_config(robot_name)) is not None + and config.home_joints is not None + for robot_name in selected_robot_names + ): options.append("Home") for attr in ("options", "values"): if hasattr(handle, attr): @@ -340,26 +607,63 @@ def _sync_preset_dropdown(self) -> None: def _apply_preset(self, preset: str) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: - return - config = self.adapter.get_robot_config(robot_name) - if config is None: + if preset not in {"Current", "Init", "Home"}: return + groups = [ + group + for group in self.adapter.list_planning_groups() + if group["id"] in self.state.selected_group_ids + ] + for group in groups: + robot_name = str(group["robot_name"]) + values_by_name = self._preset_values_by_name(preset, robot_name) + global_names = [str(name) for name in group["joint_names"]] + local_names = [str(name) for name in group["local_joint_names"]] + values = [ + float(values_by_name.get(local_name, values_by_name.get(global_name, 0.0))) + for local_name, global_name in zip(local_names, global_names, strict=False) + ] + self._set_slider_values(global_names, values) + self.state.joint_target = [float(handle.value) for handle in self._joint_sliders.values()] + self._submit_joint_target_evaluation() + self.refresh() + + def _selected_robot_names(self) -> tuple[str, ...]: + groups = self._group_info_by_id() + names: list[str] = [] + for group_id in self.state.selected_group_ids: + group = groups.get(group_id) + if group is None: + continue + robot_name = str(group["robot_name"]) + if robot_name not in names: + names.append(robot_name) + return tuple(names) + + def _preset_values_by_name(self, preset: str, robot_name: str) -> dict[str, float]: if preset == "Current": current = self.adapter.get_current_joint_state(robot_name) - values = list(current.position) if current is not None else [] - elif preset == "Init": + if current is None: + return {} + return { + str(name): float(value) + for name, value in zip(current.name, current.position, strict=False) + } + if preset == "Init": init = self.adapter.get_init_joints(robot_name) - values = list(init.position) if init is not None else [] - elif preset == "Home": - values = list(config.home_joints or []) - else: - return - self._set_slider_values(config.joint_names, values) - self.state.joint_target = [float(value) for value in values] - self._submit_joint_target_evaluation() - self.refresh() + if init is None: + return {} + return { + str(name): float(value) + for name, value in zip(init.name, init.position, strict=False) + } + config = self.adapter.get_robot_config(robot_name) + if config is None: + return {} + return { + str(name): float(value) + for name, value in zip(config.joint_names, config.home_joints or [], strict=False) + } def _set_slider_values(self, joint_names: list[str], values: list[float]) -> None: self._suppress_target_callbacks = True @@ -390,73 +694,106 @@ def _on_joint_slider_update(self, _joint_name: str) -> None: return self._submit_joint_target_evaluation() - def _on_transform_update(self, target: TransformControlsHandle) -> None: + def _on_transform_update( + self, group_id: PlanningGroupID, target: TransformControlsHandle + ) -> None: if self._closed: return - if self._suppress_target_callbacks or self.state.selected_robot is None: + if self._suppress_target_callbacks or group_id not in self.state.selected_group_ids: return pose = self._pose_from_transform_target(target) if pose is None: return self.state.cartesian_target = pose + self.state.pose_targets[group_id] = pose sequence_id = self.state.next_sequence_id() self._worker.submit( TargetEvaluationRequest( sequence_id=sequence_id, source="cartesian", - robot_name=self.state.selected_robot, - pose=pose, + group_ids=self.state.selected_group_ids, + auxiliary_group_ids=self._selected_auxiliary_group_ids(), + pose_targets=self._active_pose_targets(), + check_collision=True, ) ) self.refresh() def _submit_joint_target_evaluation(self) -> None: - robot_name = self.state.selected_robot - if robot_name is None: + targets = self._target_set_from_sliders() + if targets is None: return - target = self._target_from_sliders(robot_name) - if target is None: - return - self.state.joint_target = list(target.position) - self._move_joint_target_visuals(robot_name, target) + self.state.group_joint_targets = targets + self._refresh_target_joints_from_groups() + self._move_joint_target_visuals() sequence_id = self.state.next_sequence_id() self._worker.submit( TargetEvaluationRequest( sequence_id=sequence_id, source="joints", - robot_name=robot_name, - joints=target, + group_ids=self.state.selected_group_ids, + joint_targets=dict(targets), ) ) self.refresh() - def _move_joint_target_visuals(self, robot_name: str, target: JointState) -> None: + def _move_joint_target_visuals(self) -> None: """Optimistically move target visuals before collision/feasibility returns.""" - config = self.adapter.get_robot_config(robot_name) - robot_id = self.adapter.robot_id_for_name(robot_name) - if self.scene is not None and config is not None and robot_id is not None: - self.scene.set_target_joints(str(robot_id), config.joint_names, list(target.position)) - pose = self.adapter.get_ee_pose(robot_name, target) - if pose is not None: - self._suppress_target_callbacks = True - try: - self.scene.set_target_pose(str(robot_id), pose) - finally: - self._suppress_target_callbacks = False + if self.scene is None: + return + groups = self._group_info_by_id() + for group_id, target in self.state.group_joint_targets.items(): + group = groups.get(group_id) + if group is None: + continue + robot_name = str(group["robot_name"]) + robot_id = self.adapter.robot_id_for_name(robot_name) + config = self.adapter.get_robot_config(robot_name) + if robot_id is None or config is None: + continue + local_positions = dict(zip(target.name, target.position, strict=False)) + joints = [ + float(local_positions.get(str(global_name), 0.0)) + for global_name in group["joint_names"] + ] + self.scene.set_target_joints(str(robot_id), group["local_joint_names"], joints) + + def _sync_target_ghost_visibility(self) -> None: + if self.scene is None: + return + active_robot_ids: set[str] = set() + groups = self._group_info_by_id() + for group_id in self._selected_pose_group_ids(): + group = groups.get(group_id) + if group is None: + continue + robot_id = self.adapter.robot_id_for_name(str(group["robot_name"])) + if robot_id is not None: + active_robot_ids.add(str(robot_id)) + set_target_active = getattr(self.scene, "set_target_active", None) + if not callable(set_target_active): + return + for _robot_name, robot_id, _config in self.adapter.robot_items(): + set_target_active(str(robot_id), str(robot_id) in active_robot_ids) def _handle_target_evaluation_request( self, request: TargetEvaluationRequest - ) -> TargetEvaluation: + ) -> TargetEvaluation | TargetSetEvaluation: if request.source == "cartesian": - if request.pose is None: + if not request.pose_targets: return {"success": False, "status": "INVALID", "message": "No pose target"} - return self.adapter.evaluate_pose_target(request.pose, request.robot_name) - if request.joints is None: + return self.adapter.evaluate_pose_target_set( + request.pose_targets, + auxiliary_groups=request.auxiliary_group_ids, + seed=self.state.last_valid_target_joints, + check_collision=request.check_collision, + ) + if not request.joint_targets: return {"success": False, "status": "INVALID", "message": "No joint target"} - return self.adapter.evaluate_joint_target(request.joints, request.robot_name) + return self.adapter.evaluate_joint_target_set(request.joint_targets) def _apply_target_evaluation_result( - self, request: TargetEvaluationRequest, result: TargetEvaluation + self, request: TargetEvaluationRequest, result: TargetEvaluation | TargetSetEvaluation ) -> None: if self._closed: return @@ -470,49 +807,77 @@ def _apply_target_evaluation_result( TargetStatus.FEASIBLE if success and collision_free else TargetStatus.INFEASIBLE ) self.state.error = "" if success and collision_free else self.state.feasibility.message - if request.source == "joints": - joint_state = result.get("joint_state") - if isinstance(joint_state, JointState): - self.state.joint_target = list(joint_state.position) - if request.source == "cartesian": - joint_state = result.get("joint_state") - if isinstance(joint_state, JointState): - self.state.joint_target = list(joint_state.position) - pose = result.get("ee_pose") - if isinstance(pose, Pose): - self.state.cartesian_target = pose + target_joints = result.get("target_joints") or result.get("joint_state") + if isinstance(target_joints, JointState) and success and collision_free: + self.state.target_joints = JointState(target_joints) + self.state.last_valid_target_joints = JointState(target_joints) + self._split_target_joints_by_group(target_joints) + group_poses = result.get("group_poses", {}) + if isinstance(group_poses, dict): + self.state.group_poses = { + str(group_id): pose + for group_id, pose in group_poses.items() + if isinstance(pose, Pose) + } + if request.source == "joints" and success and collision_free: + self._sync_pose_targets_from_group_poses() + group_diagnostics = result.get("group_diagnostics", {}) + if isinstance(group_diagnostics, dict): + self.state.group_diagnostics = { + str(group_id): str(message) for group_id, message in group_diagnostics.items() + } + if request.source == "cartesian" and success and collision_free: self._sync_controls_from_targets() self._update_target_visual_state() self.refresh() def _sync_controls_from_targets(self) -> None: - robot_name = self.state.selected_robot - if robot_name is None: - return - config = self.adapter.get_robot_config(robot_name) - if config is not None and self.state.joint_target is not None: - self._set_slider_values(list(config.joint_names), list(self.state.joint_target)) - robot_id = self.adapter.robot_id_for_name(robot_name) - if self.scene is not None and robot_id is not None: - self.scene.set_target_joints( - str(robot_id), config.joint_names, self.state.joint_target - ) + if self.state.target_joints is not None: + positions_by_name = dict( + zip(self.state.target_joints.name, self.state.target_joints.position, strict=False) + ) + self._set_slider_values(list(positions_by_name), list(positions_by_name.values())) + self._move_joint_target_visuals() # Do not write the Cartesian target back into the active transform # control here. The gizmo is the source of truth for Cartesian edits; # programmatic pose writes from delayed IK results can fight fast user # dragging and make the gizmo jump back. + def _sync_pose_targets_from_group_poses(self) -> None: + groups = self._group_info_by_id() + updated_group_ids: list[PlanningGroupID] = [] + for group_id, pose in self.state.group_poses.items(): + group = groups.get(group_id) + if group is None or not bool(group["has_pose_target"]): + continue + if group_id not in self._selected_pose_group_ids(): + continue + self.state.pose_targets[group_id] = pose + updated_group_ids.append(group_id) + first_group_id = next(iter(self._selected_pose_group_ids()), None) + if first_group_id is not None: + self.state.cartesian_target = self.state.pose_targets.get(first_group_id) + self._sync_scene_target_pose_controls(updated_group_ids) + + def _sync_scene_target_pose_controls(self, group_ids: list[PlanningGroupID]) -> None: + if self.scene is None: + return + self._suppress_target_callbacks = True + try: + for group_id in group_ids: + pose = self.state.pose_targets.get(group_id) + if pose is not None: + self.scene.set_target_pose(group_id, pose) + finally: + self._suppress_target_callbacks = False + def _update_status_text(self) -> None: current = self.state.current_joints + status_label = self.state.error or self.state.module_state status = [ - "### Manipulation Panel", - f"Robot: `{self.state.selected_robot or 'none'}`", - f"Module: `{self.state.module_state}`", - f"Backend: `{self.state.backend_status.value}`", - f"Target: `{self.state.target_status.value}`", - f"Feasibility: `{self.state.feasibility.status.value}`", - f"Plan: `{self.state.plan_state.status.value}`", - f"Action: `{self.state.action_status.value}`", + "### Status", + f"**State:** {status_label}", + f"Target: `{self.state.target_status.value}` · Plan: `{self.state.plan_state.status.value}`", ] if self.state.selected_robot is not None: status.append( @@ -522,10 +887,13 @@ def _update_status_text(self) -> None: status.append(f"Current joints: `{[round(v, 3) for v in current]}`") if self.state.last_result: status.append(f"Last result: `{self.state.last_result}`") - if self.state.error: - status.append(f"Error: `{self.state.error}`") self._set_handle_value("status", "\n\n".join(status)) + def _update_target_summary(self) -> None: + self._set_handle_value( + "target_summary", f"Feasibility: `{self.state.feasibility.status.value}`" + ) + def _update_control_state(self) -> None: self._set_disabled("plan", not self.state.can_plan()) self._set_disabled("preview", not self.state.can_preview()) @@ -536,24 +904,26 @@ def _update_control_state(self) -> None: and self.state.can_execute(self.config.current_match_tolerance) ), ) - self._set_disabled("cancel", not self.state.can_cancel()) + can_cancel = self.state.can_cancel() + self._set_disabled("cancel", not can_cancel) + self._set_visible("cancel", can_cancel) self._update_target_visual_state() def _update_target_visual_state(self) -> None: - if self.scene is None or self.state.selected_robot is None: - return - robot_id = self.adapter.robot_id_for_name(self.state.selected_robot) - if robot_id is None: + if self.scene is None: return - self.scene.set_target_visual_state( - str(robot_id), self.state.feasibility.status == FeasibilityStatus.FEASIBLE - ) + for group_id in self.state.selected_group_ids: + self.scene.set_target_visual_state( + group_id, self.state.feasibility.status == FeasibilityStatus.FEASIBLE + ) def _submit_plan(self) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: + if not self.state.selected_group_ids: + self._set_recoverable_error( + "Cannot plan until target is feasible and manipulation is idle" + ) return if not self.state.can_plan(): self._set_recoverable_error( @@ -571,24 +941,26 @@ def operation() -> None: self.state.plan_state.status = PlanStatus.FAILED self._finish_operation("reset=False", clear_error=False, operation_id=operation_id) return - target = self._target_from_sliders(robot_name) - if target is None: + targets = self._target_set_from_sliders() + if targets is None: self.state.plan_state.status = PlanStatus.FAILED self._finish_operation( "plan_to_joints=False", clear_error=False, operation_id=operation_id ) return - ok = self.adapter.plan_to_joints(target, robot_name) + ok = self.adapter.plan_target_set(targets) if not self._operation_is_current(operation_id): return if ok: - path = self.adapter.get_planned_path(robot_name) self.state.plan_state.status = PlanStatus.FRESH - self.state.plan_state.robot = robot_name - self.state.plan_state.target_joints = list(target.position) + self.state.plan_state.group_ids = self.state.selected_group_ids + self.state.plan_state.robot = self.state.selected_robot + self.state.plan_state.target_joints = list( + self.state.target_joints.position if self.state.target_joints else [] + ) self.state.plan_state.target_pose = self.state.cartesian_target - self.state.plan_state.start_joints_snapshot = list(self.state.current_joints or []) - self.state.plan_state.planned_path = path + self.state.plan_state.start_joints_snapshot = self._current_snapshot_by_group() + self.state.plan_state.planned_path = None else: self.state.plan_state.status = PlanStatus.FAILED self._finish_operation(f"plan_to_joints={ok}", operation_id=operation_id) @@ -600,9 +972,6 @@ def operation() -> None: def _submit_preview(self) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: - return if not self.state.can_preview(): self._set_recoverable_error("No fresh plan to preview") return @@ -612,7 +981,7 @@ def operation() -> None: if not self._operation_is_current(operation_id): return self.state.action_status = ActionStatus.PREVIEWING - ok = self.adapter.preview_path(robot_name) + ok = self.adapter.preview_plan() self._finish_operation(f"preview={ok}", operation_id=operation_id) self._operation_worker.submit( @@ -624,9 +993,6 @@ def operation() -> None: def _submit_execute(self) -> None: if self._closed: return - robot_name = self.state.selected_robot - if robot_name is None: - return if not self.config.allow_plan_execute: self._set_recoverable_error( "Panel execution disabled; set allow_plan_execute=True to enable" @@ -644,7 +1010,7 @@ def operation() -> None: return self.state.action_status = ActionStatus.EXECUTING self.state.plan_state.status = PlanStatus.EXECUTING - ok = self.adapter.execute(robot_name) + ok = self.adapter.execute() if not self._operation_is_current(operation_id): return if not ok: @@ -745,14 +1111,22 @@ def _set_error(self, message: str) -> None: def _set_handle_value(self, key: str, value: str) -> None: handle = self._handles.get(key) - if isinstance(handle, GuiMarkdownHandle): - self._set_optional_handle_attr(handle, "value", value) + if handle is None: + return + if hasattr(handle, "content") or hasattr(handle, "value"): + attr = "content" if hasattr(handle, "content") else "value" + self._set_optional_handle_attr(handle, attr, value) 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: + handle = self._handles.get(key) + if handle is not None: + self._set_optional_handle_attr(handle, "visible", visible) + @staticmethod def _set_optional_handle_attr(handle: object, attr: str, value: object) -> None: setattr(handle, attr, value) @@ -763,7 +1137,10 @@ def _pose_from_transform_target(self, target: TransformControlsHandle) -> Pose | return Pose({"position": [px, py, pz], "orientation": [qx, qy, qz, qw]}) def _feasibility_status( - self, result: TargetEvaluation, success: bool, collision_free: bool + self, + result: TargetEvaluation | TargetSetEvaluation, + success: bool, + collision_free: bool, ) -> FeasibilityStatus: status = str(result.get("status", "")).upper() if success and collision_free: diff --git a/dimos/manipulation/visualization/viser/scene.py b/dimos/manipulation/visualization/viser/scene.py index cd50ab098e..049ffdf659 100644 --- a/dimos/manipulation/visualization/viser/scene.py +++ b/dimos/manipulation/visualization/viser/scene.py @@ -16,11 +16,16 @@ from collections.abc import Callable, Sequence from pathlib import Path +import time from typing import Protocol, TypeAlias, cast from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake -from dimos.manipulation.visualization.viser.animation import PreviewAnimator +from dimos.manipulation.visualization.viser.animation import ( + GroupPreviewAnimation, + PreviewTrack, + sampled_joint_path_frames, +) from dimos.manipulation.visualization.viser.runtime import ( VISER_INSTALL_HINT, VISER_URDF_INSTALL_HINT, @@ -31,6 +36,7 @@ try: from viser import ( + FrameHandle, GridHandle, MeshHandle, TransformControlsEvent, @@ -69,7 +75,7 @@ REFERENCE_GRID_CELL_COLOR = (44, 54, 58) REFERENCE_GRID_SECTION_COLOR = (90, 145, 165) -SceneHandle: TypeAlias = ViserUrdf | TransformControlsHandle | GridHandle | MeshHandle +SceneHandle: TypeAlias = ViserUrdf | TransformControlsHandle | GridHandle | MeshHandle | FrameHandle class _ColorHandle(Protocol): @@ -88,9 +94,11 @@ def __init__( self._configs_by_id: dict[str, RobotModelConfig] = {} self._urdfs: dict[str, ViserUrdf] = {} self._handles: dict[str, TransformControlsHandle] = {} + self._root_frames: dict[str, FrameHandle] = {} self._grid_handle: GridHandle | None = None self._grid_visible = True self._preview_visible: dict[str, bool] = {} + self._target_active: dict[str, bool] = {} self._target_tracks_current: dict[str, bool] = {} self._ensure_reference_grid() @@ -106,9 +114,17 @@ def set_reference_grid_visible(self, visible: bool) -> None: def register_robot(self, robot_id: str, config: RobotModelConfig) -> None: self._configs_by_id[robot_id] = config self._preview_visible.setdefault(robot_id, False) + self._target_active.setdefault(robot_id, False) self._target_tracks_current.setdefault(robot_id, True) self._ensure_robot_urdfs(robot_id, config) + def set_target_active(self, robot_id: str, active: bool) -> None: + """Show target ghost only when at least one group on the robot is active.""" + self._target_active[robot_id] = active + if not active: + self._target_tracks_current[robot_id] = True + self._set_target_visibility(robot_id, active) + def _ensure_reference_grid(self) -> None: try: scene = self.server.scene @@ -155,6 +171,9 @@ def dispatch(event: TransformControlsEvent) -> None: self._handles[handle_key] = handle return handle + def remove_target_controls(self, robot_id: str) -> None: + self._remove_handle(f"{robot_id}:ee_control") + def update_current_robot(self, robot_id: str, joint_state: JointState | None) -> None: config = self._configs_by_id.get(robot_id) if config is None or joint_state is None: @@ -164,7 +183,7 @@ def update_current_robot(self, robot_id: str, joint_state: JointState | None) -> self.set_urdf_joints(current, config.joint_names, joint_state.position) if self._target_tracks_current.get(robot_id, True): self._set_target_joints(robot_id, config.joint_names, joint_state.position) - self._set_target_visibility(robot_id, True) + self._set_target_visibility(robot_id, self._target_active.get(robot_id, False)) def show_preview(self, robot_id: str) -> None: """Show the transient preview-animation ghost. @@ -183,13 +202,63 @@ def animate_path(self, robot_id: str, path: Sequence[JointState], duration: floa config = self._configs_by_id.get(robot_id) if config is None: return False - self.show_preview(robot_id) + preview = GroupPreviewAnimation( + group_ids=(), + tracks=( + PreviewTrack( + robot_id=robot_id, + group_ids=(), + joint_names=tuple(config.joint_names), + path=tuple(path), + ), + ), + ) + return self.animate_preview(preview, duration) + + def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> bool: + """Animate all preview tracks with one shared group-native frame clock.""" + if not preview.tracks: + return False + frames_by_robot: dict[str, list[list[float]]] = {} + joint_names_by_robot: dict[str, tuple[str, ...]] = {} + for track in preview.tracks: + if track.robot_id not in self._configs_by_id: + return False + frames = sampled_joint_path_frames(track.path, duration, self.preview_fps) + if not frames: + return False + frames_by_robot[track.robot_id] = frames + joint_names_by_robot[track.robot_id] = track.joint_names + + frame_count = max(len(frames) for frames in frames_by_robot.values()) + if frame_count <= 0: + return False + step_delay = duration / max(frame_count - 1, 1) if duration > 0.0 else 0.0 + + robot_ids = tuple(frames_by_robot) + for robot_id in robot_ids: + self.show_preview(robot_id) try: - return PreviewAnimator( - lambda joints: self._set_preview_ghost_joints(robot_id, config.joint_names, joints) - ).animate(path, duration, self.preview_fps) + for frame_index in range(frame_count): + for robot_id in robot_ids: + frames = frames_by_robot[robot_id] + joints = self._frame_at_shared_index(frames, frame_index, frame_count) + self._set_preview_ghost_joints(robot_id, joint_names_by_robot[robot_id], joints) + if frame_index < frame_count - 1: + time.sleep(step_delay) + return True finally: - self.hide_preview(robot_id) + for robot_id in robot_ids: + self.hide_preview(robot_id) + + @staticmethod + def _frame_at_shared_index( + frames: Sequence[list[float]], frame_index: int, frame_count: int + ) -> list[float]: + if frame_count <= 1 or len(frames) == 1: + return frames[-1] + source_index = round(frame_index * (len(frames) - 1) / (frame_count - 1)) + return frames[source_index] def set_target_joints( self, robot_id: str, joint_names: Sequence[str], joints: Sequence[float] @@ -197,6 +266,7 @@ def set_target_joints( target = self._urdfs.get(f"{robot_id}:target") if target is None: return False + self._target_active[robot_id] = True self._target_tracks_current[robot_id] = False self._set_target_joints(robot_id, joint_names, joints) self._set_target_visibility(robot_id, True) @@ -252,9 +322,13 @@ def close(self) -> None: self._grid_handle = None for urdf in self._urdfs.values(): self._remove_scene_handle(urdf) + for frame in self._root_frames.values(): + self._remove_scene_handle(frame) self._urdfs.clear() + self._root_frames.clear() self._configs_by_id.clear() self._preview_visible.clear() + self._target_active.clear() self._target_tracks_current.clear() def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: @@ -264,11 +338,7 @@ def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: key = f"{robot_id}:{kind}" if key in self._urdfs: continue - root_node_name = { - "current": f"/robots/{robot_id}/current", - "target": f"/targets/{robot_id}/target", - "preview": f"/previews/{robot_id}/ghost", - }[kind] + root_node_name = self._urdf_root_node_name(robot_id, kind, config) mesh_color_override = { "current": None, "target": GOAL_ROBOT_MESH_COLOR, @@ -284,7 +354,9 @@ def _ensure_robot_urdfs(self, robot_id: str, config: RobotModelConfig) -> None: self._set_urdf_mesh_material( self._urdfs[key], GOAL_ROBOT_FEASIBLE_COLOR, GOAL_ROBOT_FEASIBLE_OPACITY ) - self._set_handle_visibility(self._urdfs[key], True) + self._set_handle_visibility( + self._urdfs[key], self._target_active.get(robot_id, False) + ) elif kind == "preview": self._set_urdf_mesh_material( self._urdfs[key], PREVIEW_ROBOT_COLOR, PREVIEW_ROBOT_OPACITY @@ -301,6 +373,64 @@ def prepared_urdf_path(self, config: RobotModelConfig) -> Path: package_paths=package_paths, xacro_args={str(key): str(value) for key, value in config.xacro_args.items()}, convert_meshes=bool(config.auto_convert_meshes), + strip_world_joint_child_link=str(config.base_link) + if bool(getattr(config, "strip_model_world_joint", False)) + else None, + ) + ) + + def _urdf_root_node_name(self, robot_id: str, kind: str, config: RobotModelConfig) -> str: + root_node_name = { + "current": f"/robots/{robot_id}/current", + "target": f"/targets/{robot_id}/target", + "preview": f"/previews/{robot_id}/ghost", + }[kind] + if not self._has_non_identity_base_pose(config): + return root_node_name + self._ensure_base_pose_frame(robot_id, kind, config) + return f"{root_node_name}/base_pose/urdf" + + def _ensure_base_pose_frame(self, robot_id: str, kind: str, config: RobotModelConfig) -> None: + key = f"{robot_id}:{kind}:base_pose" + if key in self._root_frames: + return + pose = config.base_pose + frame_name = { + "current": f"/robots/{robot_id}/current/base_pose", + "target": f"/targets/{robot_id}/target/base_pose", + "preview": f"/previews/{robot_id}/ghost/base_pose", + }[kind] + self._root_frames[key] = self.server.scene.add_frame( + frame_name, + show_axes=False, + position=( + float(pose.position.x), + float(pose.position.y), + float(pose.position.z), + ), + wxyz=( + float(pose.orientation.w), + float(pose.orientation.x), + float(pose.orientation.y), + float(pose.orientation.z), + ), + ) + + @staticmethod + def _has_non_identity_base_pose(config: RobotModelConfig) -> bool: + pose = getattr(config, "base_pose", None) + if pose is None: + return False + return any( + abs(value) > 1e-12 + for value in ( + float(pose.position.x), + float(pose.position.y), + float(pose.position.z), + float(pose.orientation.x), + float(pose.orientation.y), + float(pose.orientation.z), + float(pose.orientation.w) - 1.0, ) ) diff --git a/dimos/manipulation/visualization/viser/state.py b/dimos/manipulation/visualization/viser/state.py index b46097df95..f16e490ffb 100644 --- a/dimos/manipulation/visualization/viser/state.py +++ b/dimos/manipulation/visualization/viser/state.py @@ -21,7 +21,8 @@ import threading from typing import Literal -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from dimos.manipulation.planning.spec.models import PlanningGroupID +from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation, TargetSetEvaluation from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger @@ -92,16 +93,25 @@ class FeasibilityState: @dataclass class PanelPlanState: status: PlanStatus = PlanStatus.NONE + group_ids: tuple[PlanningGroupID, ...] = () robot: str | None = None target_pose: Pose | None = None target_joints: list[float] | None = None - start_joints_snapshot: list[float] | None = None + start_joints_snapshot: dict[PlanningGroupID, list[float]] = field(default_factory=dict) planned_path: list[JointState] | None = None @dataclass class PanelState: selected_robot: str | None = None + selected_group_ids: tuple[PlanningGroupID, ...] = () + auxiliary_group_ids: tuple[PlanningGroupID, ...] = () + pose_targets: dict[PlanningGroupID, Pose] = field(default_factory=dict) + group_joint_targets: dict[PlanningGroupID, JointState] = field(default_factory=dict) + target_joints: JointState | None = None + last_valid_target_joints: JointState | None = None + group_poses: dict[PlanningGroupID, Pose] = field(default_factory=dict) + group_diagnostics: dict[PlanningGroupID, str] = field(default_factory=dict) runtime: PanelRuntime = PanelRuntime.STOPPED backend_status: BackendConnectionStatus = BackendConnectionStatus.DISCONNECTED target_status: TargetStatus = TargetStatus.EMPTY @@ -133,9 +143,10 @@ def can_plan(self) -> bool: return ( self.runtime == PanelRuntime.RUNNING and self.backend_status == BackendConnectionStatus.READY - and self.selected_robot is not None + and bool(self.selected_group_ids) and self.action_status == ActionStatus.IDLE and self.target_status == TargetStatus.FEASIBLE + and self.target_joints is not None and self.manipulation_state in {"IDLE", "COMPLETED", "FAULT"} and self.plan_state.status != PlanStatus.PLANNING ) @@ -169,19 +180,24 @@ def can_execute( and self.target_status == TargetStatus.FEASIBLE and self.manipulation_state in {"IDLE", "COMPLETED"} and plan.status == PlanStatus.FRESH - and plan.robot == self.selected_robot - and plan.start_joints_snapshot is not None - and self.current_joints is not None + and plan.group_ids == self.selected_group_ids + and bool(plan.start_joints_snapshot) + and self.target_joints is not None ): return False - if len(plan.start_joints_snapshot) != len(self.current_joints): + if self.current_joints is None: return False - return all( - abs(expected - current) <= current_tolerance - for expected, current in zip( - plan.start_joints_snapshot, self.current_joints, strict=False + # Multi-group freshness is checked by group snapshot when available. The + # robot-level current-joint fallback preserves one-group legacy tests. + if len(plan.start_joints_snapshot) == 1: + snapshot = next(iter(plan.start_joints_snapshot.values())) + if len(snapshot) != len(self.current_joints): + return False + return all( + abs(expected - current) <= current_tolerance + for expected, current in zip(snapshot, self.current_joints, strict=False) ) - ) + return True @property def connected(self) -> bool: @@ -203,9 +219,14 @@ def module_state(self) -> str: class TargetEvaluationRequest: sequence_id: int source: PreviewSource - robot_name: str + group_ids: tuple[PlanningGroupID, ...] + robot_name: str | None = None + auxiliary_group_ids: tuple[PlanningGroupID, ...] = () pose: Pose | None = None + pose_targets: dict[PlanningGroupID, Pose] = field(default_factory=dict) joints: JointState | None = None + joint_targets: dict[PlanningGroupID, JointState] = field(default_factory=dict) + check_collision: bool = True class TargetEvaluationWorker: @@ -218,8 +239,10 @@ class TargetEvaluationWorker: def __init__( self, - handler: Callable[[TargetEvaluationRequest], TargetEvaluation], - apply_result: Callable[[TargetEvaluationRequest, TargetEvaluation], None], + handler: Callable[[TargetEvaluationRequest], TargetEvaluation | TargetSetEvaluation], + apply_result: Callable[ + [TargetEvaluationRequest, TargetEvaluation | TargetSetEvaluation], None + ], ) -> None: self._handler = handler self._apply_result = apply_result diff --git a/dimos/manipulation/visualization/viser/test_operation_worker.py b/dimos/manipulation/visualization/viser/test_operation_worker.py index ee1d5c29c0..b6d0f4a7f5 100644 --- a/dimos/manipulation/visualization/viser/test_operation_worker.py +++ b/dimos/manipulation/visualization/viser/test_operation_worker.py @@ -141,6 +141,9 @@ def cancel(self) -> bool: def plan_to_joints(self, joints: JointState, robot_name: str | None = None) -> bool: return True + def plan_target_set(self, joint_targets: dict[str, JointState]) -> bool: + return True + def test_operation_worker_uses_per_operation_timeout() -> None: errors: list[str] = [] @@ -205,7 +208,8 @@ def test_gui_only_preview_submits_timeout_override(monkeypatch: pytest.MonkeyPat monkeypatch.setattr(gui, "_operation_worker", FakeTimeoutSubmitWorker(submissions)) gui.state.runtime = PanelRuntime.RUNNING gui.state.backend_status = BackendConnectionStatus.READY - gui.state.selected_robot = "arm" + gui.state.selected_group_ids = ("arm:manipulator",) + gui.state.target_joints = JointState({"name": ["arm/j1"], "position": [1.0]}) gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "IDLE" diff --git a/dimos/manipulation/visualization/viser/test_state.py b/dimos/manipulation/visualization/viser/test_state.py index 412c227050..040ae6f56e 100644 --- a/dimos/manipulation/visualization/viser/test_state.py +++ b/dimos/manipulation/visualization/viser/test_state.py @@ -20,11 +20,14 @@ PanelState, TargetStatus, ) +from dimos.msgs.sensor_msgs.JointState import JointState def test_panel_can_plan_from_fault_after_planning_failure() -> None: state = PanelState( selected_robot="arm", + selected_group_ids=("arm:manipulator",), + target_joints=JointState({"name": ["arm/j1"], "position": [1.0]}), runtime=PanelRuntime.RUNNING, backend_status=BackendConnectionStatus.READY, target_status=TargetStatus.FEASIBLE, diff --git a/dimos/manipulation/visualization/viser/test_viser_visualization.py b/dimos/manipulation/visualization/viser/test_viser_visualization.py index 3be6162a67..1c12ee0cb8 100644 --- a/dimos/manipulation/visualization/viser/test_viser_visualization.py +++ b/dimos/manipulation/visualization/viser/test_viser_visualization.py @@ -25,15 +25,23 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") -from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation +from dimos.manipulation.visualization.types import RobotInfo, TargetEvaluation, TargetSetEvaluation +from dimos.manipulation.visualization.viser import scene as scene_module from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter from dimos.manipulation.visualization.viser.animation import ( + GroupPreviewAnimation, PreviewAnimator, + PreviewTrack, interpolate_joint_path, sampled_joint_path_frames, ) from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig -from dimos.manipulation.visualization.viser.gui import ViserPanelGui +from dimos.manipulation.visualization.viser.gui import ( + ACTIVE_GROUP_COLOR, + INACTIVE_GROUP_COLOR, + PRIMARY_ACTION_COLOR, + ViserPanelGui, +) from dimos.manipulation.visualization.viser.scene import ViserManipulationScene from dimos.manipulation.visualization.viser.state import ( ActionStatus, @@ -47,11 +55,15 @@ ) from dimos.manipulation.visualization.viser.theme import _dimos_logo_data_url, apply_dimos_theme from dimos.msgs.geometry_msgs.Pose import Pose +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.JointState import JointState GuiCallback = Callable[[SimpleNamespace], None] ThemeValue = str | bool | tuple[int, int, int] | dict[str, str | dict[str, str]] | None RobotConfigOverride = str | list[str] | list[float] | None +DEFAULT_GROUP_ID = "arm:manipulator" @dataclass @@ -120,6 +132,7 @@ def remove(self) -> None: class GuiButtonHandle: label: str disabled: bool = False + color: tuple[int, int, int] | None = None click_callback: GuiCallback | None = None removed: bool = False @@ -166,7 +179,7 @@ def __init__(self) -> None: self.visible: object | None = None self.removed = False self.name = "" - self.kwargs: dict[str, float | bool] = {} + self.kwargs: dict[str, object] = {} def remove(self) -> None: self.removed = True @@ -206,6 +219,8 @@ class FakeServer: def __init__(self) -> None: self.scene = SimpleNamespace() self.scene.add_transform_controls = self.add_transform_controls + self.scene.add_frame = self.add_frame + self.frames = [] def add_transform_controls(self, path: str, *, scale: float) -> FakeTransformHandle: handle = FakeTransformHandle() @@ -213,6 +228,13 @@ def add_transform_controls(self, path: str, *, scale: float) -> FakeTransformHan handle.scale = scale return handle + def add_frame(self, name: str, **kwargs: object) -> FakeHandle: + handle = FakeHandle() + handle.name = name + handle.kwargs = kwargs + self.frames.append(handle) + return handle + class FakeGridServer(FakeServer): def __init__(self) -> None: @@ -223,7 +245,7 @@ def __init__(self) -> None: def add_grid(self, name: str, **kwargs: float | bool) -> FakeHandle: handle = FakeHandle() handle.name = name - handle.kwargs = kwargs + handle.kwargs = dict(kwargs) handle.visible = kwargs.get("visible") self.grids.append(handle) return handle @@ -314,8 +336,16 @@ def add_dropdown( handle = GuiDropdownHandle(label=label, options=list(options), value=initial_value) return handle - def add_button(self, label: str, *, disabled: bool = False) -> GuiButtonHandle: - handle = GuiButtonHandle(label=label, disabled=disabled) + def add_button( + self, + label: str, + *, + disabled: bool = False, + color: tuple[int, int, int] | None = None, + hint: str | None = None, + ) -> GuiButtonHandle: + _ = hint + handle = GuiButtonHandle(label=label, disabled=disabled, color=color) self.buttons[label] = handle return handle @@ -346,6 +376,27 @@ def make_robot_config(**overrides: RobotConfigOverride) -> RobotConfigStub: return config +def make_planning_group_info( + robot_name: str, + config: RobotConfigStub | SimpleNamespace, + *, + group_name: str = "manipulator", + has_pose_target: bool = True, +) -> dict[str, object]: + joint_names = [str(name) for name in config.joint_names] + return { + "id": f"{robot_name}:{group_name}", + "name": group_name, + "robot_name": robot_name, + "joint_names": [f"{robot_name}/{name}" for name in joint_names], + "local_joint_names": joint_names, + "base_link": str(config.base_link), + "tip_link": str(config.end_effector_link) if has_pose_target else None, + "has_pose_target": has_pose_target, + "source": "fallback", + } + + class FakeManipulationModule(SimpleNamespace): """Public ManipulationModule surface used by the in-process Viser adapter tests.""" @@ -378,10 +429,18 @@ def get_robot_info(self, robot_name: str) -> RobotInfo | None: return None init = self.get_init_joints(robot_name) home_joints = config.home_joints if hasattr(config, "home_joints") else None + planning_groups = getattr(self, "_planning_groups", None) + if planning_groups is None: + planning_groups = [make_planning_group_info(robot_name, config)] + else: + planning_groups = [ + group for group in planning_groups if str(group["robot_name"]) == robot_name + ] return { "name": config.name, "world_robot_id": self.robot_id_for_name(robot_name) or robot_name, "joint_names": list(config.joint_names), + "planning_groups": planning_groups, "end_effector_link": config.end_effector_link, "base_link": config.base_link, "max_velocity": 1.0, @@ -396,13 +455,6 @@ def get_robot_info(self, robot_name: str) -> RobotInfo | None: def get_init_joints(self, robot_name: str) -> JointState | None: return getattr(self, "_init_joints", {}).get(robot_name) - def get_planned_path(self, robot_name: str) -> list[JointState] | None: - return getattr(self, "_planned_paths", {}).get(robot_name) - - def get_planned_trajectory_duration(self, robot_name: str) -> float | None: - trajectory = getattr(self, "_planned_trajectories", {}).get(robot_name) - return None if trajectory is None else float(trajectory.duration) - def get_state(self) -> str: state = getattr(self, "_state", "IDLE") return str(getattr(state, "name", state)) @@ -427,7 +479,9 @@ def evaluate_joint_target(self, joints: JointState | None, robot_name: str) -> T "joint_state": joints, } - def evaluate_pose_target(self, _pose: Pose, _robot_name: str) -> TargetEvaluation: + def evaluate_pose_target( + self, _pose: Pose, _robot_name: str, *, check_collision: bool = True + ) -> TargetEvaluation: return { "success": False, "joint_state": None, @@ -436,6 +490,57 @@ def evaluate_pose_target(self, _pose: Pose, _robot_name: str) -> TargetEvaluatio "collision_free": False, } + def evaluate_pose_target_set( + self, + pose_targets: dict[str, Pose], + auxiliary_groups: Sequence[str] = (), + seed: JointState | None = None, + check_collision: bool = True, + ) -> TargetSetEvaluation: + target = JointState({"name": ["arm/j1", "arm/j2"], "position": [0.1, 0.2]}) + return { + "success": True, + "status": "FEASIBLE", + "message": "Target collision check skipped" + if not check_collision + else "Target is collision-free", + "collision_free": True, + "target_joints": target, + "group_ids": tuple(pose_targets) + tuple(auxiliary_groups), + "group_poses": dict(pose_targets), + } + + def evaluate_joint_target_set( + self, joint_targets: dict[str, JointState] + ) -> TargetSetEvaluation: + if not joint_targets: + return {"success": False, "status": "INVALID", "target_joints": None} + names: list[str] = [] + positions: list[float] = [] + collision_free = True + group_poses = {} + world_monitor = getattr(self, "_world_monitor", None) + for group_id, target in joint_targets.items(): + robot_name = group_id.split(":", 1)[0] + robot_id = self.robot_id_for_name(robot_name) + names.extend(target.name) + positions.extend(float(value) for value in target.position) + if world_monitor is not None and robot_id is not None: + collision_free = collision_free and world_monitor.is_state_valid(robot_id, target) + group_poses[group_id] = world_monitor.get_ee_pose(robot_id, target) + return { + "success": True, + "status": "FEASIBLE" if collision_free else "COLLISION", + "message": "Target is collision-free" if collision_free else "Target is in collision", + "collision_free": collision_free, + "target_joints": JointState({"name": names, "position": positions}), + "group_ids": tuple(joint_targets), + "group_poses": group_poses, + } + + def plan_to_joint_targets(self, _joint_targets: dict[str, JointState]) -> bool: + return True + def make_adapter_with_robot() -> InProcessViserAdapter: current = FakeJointState(["j1", "j2"], position=[0.3, 0.4]) @@ -455,8 +560,6 @@ def make_adapter_with_robot() -> InProcessViserAdapter: module = FakeManipulationModule( _robots={"arm": ("robot-1", config, None)}, _init_joints={"arm": FakeJointState(["j1", "j2"], position=[0.1, 0.2])}, - _planned_paths={}, - _planned_trajectories={}, _state=NamedState(name="IDLE"), _error_message="", _world_monitor=world_monitor, @@ -504,8 +607,44 @@ def test_gui_builds_controls_in_manipulation_panel_folder( assert server.folders[0].label == "Manipulation Panel" assert server.folders[0].kwargs == {"expand_by_default": True} assert "status" in gui._handles - assert "robot" in gui._handles + assert "robot" not in gui._handles + assert "planning_groups_heading" in gui._handles + assert "target_heading" in gui._handles + assert "target_summary" in gui._handles + assert "actions_heading" in gui._handles assert "plan" in gui._handles + assert "select_all_manipulators" not in gui._handles + assert "clear_group_selection" not in gui._handles + assert "plan_controls_heading" in gui._handles + assert "actions_folder" not in gui._handles + assert "joint_control_folder" in gui._handles + handle_order = list(gui._handles) + assert handle_order.index(f"group:{DEFAULT_GROUP_ID}") < handle_order.index("plan") + assert handle_order.index("target_summary") < handle_order.index("plan") + assert handle_order.index("plan") < handle_order.index("plan_controls_heading") + assert handle_order.index("plan_controls_heading") < handle_order.index("preview") + assert handle_order.index("preview") < handle_order.index("execute") + assert handle_order.index("clear") < handle_order.index("joint_control_folder") + assert isinstance(gui._handles["status"], GuiMarkdownHandle) + assert "Starting" not in gui._handles["status"].value + assert isinstance(gui._handles["target_summary"], GuiMarkdownHandle) + assert "Feasibility:" in gui._handles["target_summary"].value + assert "Primary:" not in gui._handles["target_summary"].value + assert "Auxiliary:" not in gui._handles["target_summary"].value + assert "Ghosts:" not in gui._handles["target_summary"].value + assert isinstance(gui._handles["plan_controls_heading"], GuiMarkdownHandle) + assert "Plan controls" in gui._handles["plan_controls_heading"].value + plan_button = gui._handles["plan"] + assert isinstance(plan_button, GuiButtonHandle) + assert plan_button.color == PRIMARY_ACTION_COLOR + group_button = gui._handles[f"group:{DEFAULT_GROUP_ID}"] + assert isinstance(group_button, GuiButtonHandle) + assert group_button.label == "arm" + assert group_button.color == ACTIVE_GROUP_COLOR + joint_folder = gui._handles["joint_control_folder"] + assert isinstance(joint_folder, FakeFolder) + assert joint_folder.label == "Joint Control" + assert joint_folder.kwargs == {"expand_by_default": False} assert gui._operation_worker._timeout_seconds is None @@ -541,14 +680,11 @@ def test_gui_close_removes_handles_and_late_callbacks_are_noops( ) adapter = make_adapter_with_robot() gui = make_panel(server, adapter, ViserVisualizationConfig(), scene) - robot_dropdown = gui._handles["robot"] plan_button = server.buttons["Plan"] grid = grid_server.grids[0] handles = list(gui._handles.values()) gui.close() - if isinstance(robot_dropdown, GuiDropdownHandle) and robot_dropdown.update_callback is not None: - robot_dropdown.update_callback(SimpleNamespace(target=SimpleNamespace(value="arm"))) if plan_button.click_callback is not None: plan_button.click_callback(SimpleNamespace()) gui._set_scene_grid_visible(False) @@ -568,8 +704,8 @@ def test_gui_ignores_target_evaluation_after_close( request = TargetEvaluationRequest( sequence_id=sequence_id, source="joints", - robot_name="arm", - joints=FakeJointState(["j1", "j2"], position=[0.1, 0.2]), + group_ids=(DEFAULT_GROUP_ID,), + joint_targets={DEFAULT_GROUP_ID: FakeJointState(["arm/j1", "arm/j2"], position=[0.1, 0.2])}, ) gui.close() @@ -592,12 +728,12 @@ def test_dimos_theme_configures_supported_viser_chrome() -> None: assert apply_dimos_theme(server) is True assert server.theme_kwargs is not None - assert server.theme_kwargs["brand_color"] == (22, 130, 163) + assert server.theme_kwargs["brand_color"] == (0, 153, 255) assert server.theme_kwargs["dark_mode"] is True assert server.theme_kwargs["show_logo"] is False assert server.theme_kwargs["show_share_button"] is False - assert server.theme_kwargs["control_layout"] == "collapsible" - assert server.theme_kwargs["control_width"] == "medium" + assert server.theme_kwargs["control_layout"] == "fixed" + assert server.theme_kwargs["control_width"] == "large" def test_dimos_theme_configures_titlebar_when_supported(monkeypatch: pytest.MonkeyPatch) -> None: @@ -737,8 +873,10 @@ def test_preview_visibility_only_affects_preview_ghost_and_close_removes_handles scene.register_robot("robot1", config) target = scene._urdfs["robot1:target"] preview = scene._urdfs["robot1:preview"] - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) assert all(mesh.visible is False for mesh in preview._meshes) + scene.set_target_active("robot1", True) + assert all(mesh.visible is True for mesh in target._meshes) scene.show_preview("robot1") assert all(mesh.visible is True for mesh in preview._meshes) assert all(mesh.visible is True for mesh in target._meshes) @@ -750,7 +888,7 @@ def test_preview_visibility_only_affects_preview_ghost_and_close_removes_handles assert all(mesh.visible is False for mesh in preview._meshes) -def test_target_ghost_is_visible_and_tracks_current_until_target_moves_it() -> None: +def test_target_ghost_tracks_current_but_is_visible_only_when_active() -> None: server = FakeServer() urdfs = [FakeViserUrdfWithMeshes(("joint1",)) for _ in range(3)] scene = ViserManipulationScene(server, lambda *args, **kwargs: urdfs.pop(0), preview_fps=10.0) @@ -768,12 +906,16 @@ def test_target_ghost_is_visible_and_tracks_current_until_target_moves_it() -> N target = scene._urdfs["robot1:target"] preview = scene._urdfs["robot1:preview"] - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) assert all(mesh.visible is False for mesh in preview._meshes) scene.update_current_robot("robot1", FakeJointState(["joint1"], position=[0.25])) assert current.cfg == [0.25] assert target.cfg == [0.25] assert preview.cfg is None + assert all(mesh.visible is False for mesh in target._meshes) + + scene.set_target_active("robot1", True) + assert all(mesh.visible is True for mesh in target._meshes) scene.set_target_joints("robot1", ["joint1"], [0.8]) scene.update_current_robot("robot1", FakeJointState(["joint1"], position=[0.1])) @@ -781,6 +923,51 @@ def test_target_ghost_is_visible_and_tracks_current_until_target_moves_it() -> N assert target.cfg == [0.8] assert preview.cfg is None + scene.set_target_active("robot1", False) + assert all(mesh.visible is False for mesh in target._meshes) + + +def test_scene_parents_urdfs_under_base_pose_frame() -> None: + server = FakeServer() + root_node_names: list[str] = [] + + def make_urdf(*_: object, **kwargs: object) -> FakeViserUrdfWithMeshes: + root_node_names.append(str(kwargs["root_node_name"])) + return FakeViserUrdfWithMeshes(("joint1",)) + + scene = ViserManipulationScene(server, make_urdf, preview_fps=10.0) + scene.prepared_urdf_path = lambda config: "dummy.urdf" + config = SimpleNamespace( + name="arm", + model_path="/tmp/arm.urdf", + package_paths={}, + xacro_args={}, + auto_convert_meshes=False, + joint_names=["joint1"], + base_pose=PoseStamped( + position=Vector3(1.0, 2.0, 3.0), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + ), + ) + + scene.register_robot("robot1", config) + + assert [frame.name for frame in server.frames] == [ + "/robots/robot1/current/base_pose", + "/targets/robot1/target/base_pose", + "/previews/robot1/ghost/base_pose", + ] + assert [frame.kwargs["position"] for frame in server.frames] == [ + (1.0, 2.0, 3.0), + (1.0, 2.0, 3.0), + (1.0, 2.0, 3.0), + ] + assert root_node_names == [ + "/robots/robot1/current/base_pose/urdf", + "/targets/robot1/target/base_pose/urdf", + "/previews/robot1/ghost/base_pose/urdf", + ] + def test_preview_animation_uses_separate_colored_ghost_and_hides_after_playback() -> None: server = FakeServer() @@ -815,7 +1002,74 @@ def test_preview_animation_uses_separate_colored_ghost_and_hides_after_playback( assert ok is True assert preview.cfg == [1.0] assert all(mesh.visible is False for mesh in preview._meshes) - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) + + +def test_group_preview_animation_updates_all_tracks_on_shared_frame_clock( + monkeypatch: pytest.MonkeyPatch, +) -> None: + server = FakeServer() + scene = ViserManipulationScene( + server, lambda *args, **kwargs: FakeUrdf(("joint1",)), preview_fps=10.0 + ) + scene.prepared_urdf_path = lambda config: "dummy.urdf" + config = SimpleNamespace( + name="arm", + model_path="/tmp/arm.urdf", + package_paths={}, + xacro_args={}, + auto_convert_meshes=False, + joint_names=["joint1"], + ) + scene.register_robot("left", config) + scene.register_robot("right", config) + updates: list[tuple[str, tuple[str, ...], tuple[float, ...]]] = [] + sleep_calls: list[float] = [] + + def record_preview_joints( + robot_id: str, joint_names: Sequence[str], joints: Sequence[float] + ) -> None: + updates.append((robot_id, tuple(joint_names), tuple(joints))) + + monkeypatch.setattr(scene, "_set_preview_ghost_joints", record_preview_joints) + monkeypatch.setattr(scene_module.time, "sleep", sleep_calls.append) + + ok = scene.animate_preview( + GroupPreviewAnimation( + group_ids=("left/arm", "right/arm"), + tracks=( + PreviewTrack( + robot_id="left", + group_ids=("left/arm",), + joint_names=("joint1",), + path=( + FakeJointState(["joint1"], position=[0.0]), + FakeJointState(["joint1"], position=[1.0]), + ), + ), + PreviewTrack( + robot_id="right", + group_ids=("right/arm",), + joint_names=("joint1",), + path=( + FakeJointState(["joint1"], position=[10.0]), + FakeJointState(["joint1"], position=[11.0]), + ), + ), + ), + ), + duration=0.0, + ) + + assert ok is True + assert updates == [ + ("left", ("joint1",), (0.0,)), + ("right", ("joint1",), (10.0,)), + ("left", ("joint1",), (1.0,)), + ("right", ("joint1",), (11.0,)), + ] + assert sleep_calls == [0.0] + assert scene._preview_visible == {"left": False, "right": False} def test_scene_target_helpers_handle_missing_robot_and_pose() -> None: @@ -903,11 +1157,9 @@ def test_adapter_copies_joint_state_and_delegates_to_module() -> None: copied = FakeJointState(["j1"], position=[1.0], velocity=[2.0], effort=[3.0]) module = FakeManipulationModule( _robots={"arm": ("robot-1", SimpleNamespace(), None)}, - _planned_paths={"arm": [copied]}, - _planned_trajectories={}, plan_to_pose=lambda pose, robot_name=None: (pose, robot_name), plan_to_joints=lambda joints, robot_name=None: (joints, robot_name), - preview_path=lambda robot_name=None: robot_name, + preview_plan=lambda robot_name=None: robot_name, execute=lambda robot_name=None: robot_name, cancel=lambda: True, clear_planned_path=lambda: True, @@ -921,19 +1173,13 @@ def test_adapter_copies_joint_state_and_delegates_to_module() -> None: module._world_monitor = world_monitor adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) - planned = adapter.get_planned_path("arm") - assert planned is not None - assert planned[0] is not copied - assert planned[0].name is not copied.name - assert planned[0].position is not copied.position - current = adapter.get_current_joint_state("arm") assert current is not copied assert current.name is not copied.name assert adapter.plan_to_pose("pose", "arm") == ("pose", "arm") - assert adapter.preview_path("arm") == "arm" - assert adapter.evaluate_joint_target(planned[0], "arm")["status"] == "FEASIBLE" + assert adapter.preview_plan("arm") == "arm" + assert adapter.evaluate_joint_target(copied, "arm")["status"] == "FEASIBLE" def test_adapter_evaluate_joint_target_uses_world_monitor_and_copies_input() -> None: @@ -1028,7 +1274,7 @@ def test_scene_registers_goal_robot_coloring_and_updates_visibility() -> None: assert all(mesh.visible is True for mesh in preview._meshes) scene.hide_preview("robot1") assert all(mesh.visible is False for mesh in preview._meshes) - assert all(mesh.visible is True for mesh in target._meshes) + assert all(mesh.visible is False for mesh in target._meshes) def test_scene_transform_controls_update_pose_callback_and_visual_state() -> None: @@ -1112,9 +1358,7 @@ def test_gui_initializes_pose_selector_to_current_ee_pose( orientation=SimpleNamespace(w=0.9, x=0.1, y=0.2, z=0.3), ) config = make_robot_config(joint_names=["j1"], home_joints=[0.0]) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} - ) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, @@ -1125,12 +1369,136 @@ def test_gui_initializes_pose_selector_to_current_ee_pose( FakeTransformServer(), lambda *args, **kwargs: FakeViserUrdfWithMeshes(), preview_fps=10.0 ) gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) - control = scene._handles["robot-1:ee_control"] + control = scene._handles[f"{DEFAULT_GROUP_ID}:ee_control"] assert control.position == (0.1, 0.2, 0.3) assert control.wxyz == (0.9, 0.1, 0.2, 0.3) assert gui.state.cartesian_target is current_pose +def test_gui_removes_pose_selector_when_group_is_deselected( + make_panel: Callable[..., ViserPanelGui], +) -> None: + current = FakeJointState(["j1"], position=[0.25]) + current_pose = SimpleNamespace( + position=SimpleNamespace(x=0.1, y=0.2, z=0.3), + orientation=SimpleNamespace(w=1.0, x=0.0, y=0.0, z=0.0), + ) + config = make_robot_config(joint_names=["j1"], home_joints=[0.0]) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + get_ee_pose=lambda robot_id, joint_state=None: current_pose, + ) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + scene = ViserManipulationScene( + FakeTransformServer(), lambda *args, **kwargs: FakeViserUrdfWithMeshes(), preview_fps=10.0 + ) + gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) + control = scene._handles[f"{DEFAULT_GROUP_ID}:ee_control"] + + gui._set_group_selected(DEFAULT_GROUP_ID, False) + + assert f"{DEFAULT_GROUP_ID}:ee_control" not in scene._handles + assert control.removed is True + + +def test_gui_group_selector_derives_primary_and_auxiliary_groups( + make_panel: Callable[..., ViserPanelGui], +) -> None: + current = FakeJointState(["j1", "grip"], position=[0.25, 0.5]) + config = make_robot_config(joint_names=["j1", "grip"], home_joints=[0.0, 0.0]) + pose_group = make_planning_group_info("arm", config) + auxiliary_group = make_planning_group_info( + "arm", config, group_name="gripper", has_pose_target=False + ) + module = FakeManipulationModule( + _robots={"arm": ("robot-1", config, None)}, + _planning_groups=[pose_group, auxiliary_group], + ) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=lambda robot_id, joint_state: True, + get_ee_pose=lambda robot_id, joint_state=None: Pose( + {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} + ), + ) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + target_controls = [] + scene = SimpleNamespace( + has_reference_grid=lambda: False, + ensure_target_controls=lambda *args: target_controls.append(args) or object(), + remove_target_controls=lambda *args: None, + set_target_active=lambda *args: None, + set_target_joints=lambda *args: True, + set_target_pose=lambda *args: None, + set_target_visual_state=lambda *args: None, + ) + server = FakeGuiServer() + + gui = make_panel(server, adapter, ViserVisualizationConfig(panel_enabled=True), scene) + assert "robot" not in gui._handles + pose_button = gui._handles["group:arm:manipulator"] + aux_button = gui._handles["group:arm:gripper"] + assert isinstance(pose_button, GuiButtonHandle) + assert isinstance(aux_button, GuiButtonHandle) + assert pose_button.label == "arm" + assert pose_button.color == ACTIVE_GROUP_COLOR + assert aux_button.label == "arm gripper" + assert aux_button.color == INACTIVE_GROUP_COLOR + + assert aux_button.click_callback is not None + aux_button.click_callback(SimpleNamespace(target=aux_button)) + + assert gui.state.selected_group_ids == ("arm:manipulator", "arm:gripper") + assert gui.state.auxiliary_group_ids == ("arm:gripper",) + assert aux_button.label == "arm gripper" + assert aux_button.color == ACTIVE_GROUP_COLOR + assert [call[0] for call in target_controls] == ["arm:manipulator"] + + +def test_gui_target_ghost_visibility_follows_active_selected_groups( + make_panel: Callable[..., ViserPanelGui], +) -> None: + left_config = make_robot_config(name="left", joint_names=["j1"], home_joints=[0.0]) + right_config = make_robot_config(name="right", joint_names=["j1"], home_joints=[0.0]) + module = FakeManipulationModule( + _robots={ + "left": ("left-id", left_config, None), + "right": ("right-id", right_config, None), + } + ) + current = FakeJointState(["j1"], position=[0.0]) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=lambda robot_id, joint_state: True, + get_ee_pose=lambda robot_id, joint_state=None: Pose( + {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} + ), + ) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + active_updates = [] + scene = SimpleNamespace( + has_reference_grid=lambda: False, + ensure_target_controls=lambda *args: object(), + remove_target_controls=lambda *args: None, + set_target_active=lambda *args: active_updates.append(args), + set_target_joints=lambda *args: True, + set_target_pose=lambda *args: None, + set_target_visual_state=lambda *args: None, + ) + + gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) + + assert active_updates[-2:] == [("left-id", True), ("right-id", False)] + gui._set_group_selected("right:manipulator", True) + assert active_updates[-2:] == [("left-id", True), ("right-id", True)] + gui._set_group_selected("left:manipulator", False) + assert active_updates[-2:] == [("left-id", False), ("right-id", True)] + + def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callbacks( make_panel: Callable[..., ViserPanelGui], ) -> None: @@ -1139,8 +1507,6 @@ def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callback module = FakeManipulationModule( _robots={"arm": ("robot-1", config, None)}, _init_joints={"arm": FakeJointState(["j1", "j2"], position=[-1.0, -2.0])}, - _planned_paths={}, - _planned_trajectories={}, ) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, @@ -1151,11 +1517,11 @@ def test_gui_preset_dropdown_and_controls_include_init_home_current_and_callback adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) gui = make_panel(FakeGuiServer(), adapter) assert gui._handles["preset"].options == ["Select preset...", "Init", "Current", "Home"] - assert list(gui._joint_sliders) == ["j1", "j2"] + assert list(gui._joint_sliders) == ["arm/j1", "arm/j2"] gui._apply_preset("Home") - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [1.0, 2.0] gui._apply_preset("Current") - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [0.25, 0.5] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [0.25, 0.5] gui._submit_execute() assert gui.state.error == "Panel execution disabled; set allow_plan_execute=True to enable" @@ -1165,9 +1531,7 @@ def test_gui_rebuilding_joint_sliders_removes_stale_viser_handles( ) -> None: current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) config = make_robot_config(joint_names=["j1", "j2"], home_joints=[1.0, 2.0]) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} - ) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, @@ -1181,10 +1545,12 @@ def test_gui_rebuilding_joint_sliders_removes_stale_viser_handles( assert [slider.value for slider in stale_sliders] == [0.0, 0.0] current.position = [-0.738, -0.2826151825863572] + gui.state.target_joints = None + gui.state.group_joint_targets.clear() gui._build_joint_sliders() assert all(slider.removed is True for slider in stale_sliders) - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [ + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [ -0.738, -0.2826151825863572, ] @@ -1212,8 +1578,6 @@ def test_panel_execution_is_gated_by_default_and_refresh_updates_robot_controls( config = make_robot_config(joint_names=["j1"], home_joints=[0.5]) module = FakeManipulationModule( _robots={"arm": ("robot-1", config, None)}, - _planned_paths={}, - _planned_trajectories={}, execute=lambda robot_name=None: False, ) world_monitor = SimpleNamespace( @@ -1229,9 +1593,9 @@ def test_panel_execution_is_gated_by_default_and_refresh_updates_robot_controls( gui = make_panel(FakeGuiServer(), adapter) gui.refresh() assert gui.state.selected_robot == "arm" - assert list(gui._joint_sliders) == ["j1"] + assert list(gui._joint_sliders) == ["arm/j1"] gui._apply_preset("Home") - assert gui._joint_sliders["j1"].value == 0.5 + assert gui._joint_sliders["arm/j1"].value == 0.5 gui._submit_execute() assert "Panel execution disabled" in gui.state.error @@ -1243,9 +1607,7 @@ def test_gui_moves_joint_target_immediately_and_stores_evaluated_joint_solution( current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) target_pose = SimpleNamespace(position=SimpleNamespace(x=0.2, y=0.3, z=0.4)) config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.5, 0.6]) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} - ) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, @@ -1268,39 +1630,48 @@ def test_gui_moves_joint_target_immediately_and_stores_evaluated_joint_solution( gui._worker = SimpleNamespace( submit=lambda request: requests.append(request), stop=lambda: None ) - gui._joint_sliders["j1"].value = 0.25 - gui._joint_sliders["j2"].value = 0.75 + gui._joint_sliders["arm/j1"].value = 0.25 + gui._joint_sliders["arm/j2"].value = 0.75 gui._submit_joint_target_evaluation() assert target_updates[-1] == ("robot-1", ["j1", "j2"], [0.25, 0.75]) - assert target_pose_updates[-1] == ("robot-1", target_pose) + assert target_pose_updates[-1] == (DEFAULT_GROUP_ID, target_pose) assert requests[-1].source == "joints" - stale_request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") - fresh_request = TargetEvaluationRequest(sequence_id=2, source="joints", robot_name="arm") + stale_request = TargetEvaluationRequest( + sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) + fresh_request = TargetEvaluationRequest( + sequence_id=2, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) gui.state.latest_sequence_id = 2 gui._apply_target_evaluation_result( stale_request, { "success": True, "collision_free": True, - "joint_state": adapter.joints_from_values(["j1", "j2"], [9.0, 9.0]), + "target_joints": adapter.joints_from_values(["arm/j1", "arm/j2"], [9.0, 9.0]), }, ) - assert gui.state.joint_target == [0.25, 0.75] + assert gui.state.target_joints is not None + assert list(gui.state.target_joints.position) == [0.25, 0.75] + joint_bar_pose = Pose({"position": [0.4, 0.5, 0.6], "orientation": [0.0, 0.0, 0.0, 1.0]}) gui._apply_target_evaluation_result( fresh_request, { "success": True, "collision_free": True, - "joint_state": adapter.joints_from_values(["j1", "j2"], [1.0, 2.0]), + "target_joints": adapter.joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), + "group_poses": {DEFAULT_GROUP_ID: joint_bar_pose}, }, ) assert gui.state.target_status == TargetStatus.FEASIBLE assert gui.state.feasibility.status == FeasibilityStatus.FEASIBLE - assert gui.state.joint_target == [1.0, 2.0] - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [0.25, 0.75] + assert gui.state.target_joints is not None + assert list(gui.state.target_joints.position) == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [0.25, 0.75] assert target_updates[-1] == ("robot-1", ["j1", "j2"], [0.25, 0.75]) + assert target_pose_updates[-1] == (DEFAULT_GROUP_ID, joint_bar_pose) def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( @@ -1308,9 +1679,7 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( ) -> None: current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.5, 0.6]) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} - ) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, @@ -1322,16 +1691,21 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( target_pose_updates = [] scene = SimpleNamespace( has_reference_grid=lambda: False, - ensure_target_controls=lambda *args: None, + ensure_target_controls=lambda *args: object(), set_target_joints=lambda *args: target_joint_updates.append(args) or True, set_target_pose=lambda *args: target_pose_updates.append(args), set_target_visual_state=lambda *args: None, ) gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) - gui.state.cartesian_target = Pose( - {"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]} + gui._handles[f"ee_control:{DEFAULT_GROUP_ID}"] = object() + dragged_pose = Pose({"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]}) + solved_pose = Pose({"position": [0.4, 0.5, 0.6], "orientation": [0.0, 0.0, 0.0, 1.0]}) + gui.state.cartesian_target = dragged_pose + gui.state.pose_targets[DEFAULT_GROUP_ID] = dragged_pose + target_pose_updates.clear() + request = TargetEvaluationRequest( + sequence_id=1, source="cartesian", group_ids=(DEFAULT_GROUP_ID,) ) - request = TargetEvaluationRequest(sequence_id=1, source="cartesian", robot_name="arm") gui.state.latest_sequence_id = 1 gui._apply_target_evaluation_result( @@ -1339,14 +1713,64 @@ def test_gui_cartesian_ik_result_does_not_rewrite_active_gizmo( { "success": True, "collision_free": True, - "joint_state": adapter.joints_from_values(["j1", "j2"], [1.0, 2.0]), + "target_joints": adapter.joints_from_values(["arm/j1", "arm/j2"], [1.0, 2.0]), + "group_poses": {DEFAULT_GROUP_ID: solved_pose}, }, ) assert gui.state.target_status == TargetStatus.FEASIBLE - assert [gui._joint_sliders[name].value for name in ("j1", "j2")] == [1.0, 2.0] + assert [gui._joint_sliders[name].value for name in ("arm/j1", "arm/j2")] == [1.0, 2.0] assert target_joint_updates[-1] == ("robot-1", ["j1", "j2"], [1.0, 2.0]) assert target_pose_updates == [] + assert gui.state.pose_targets[DEFAULT_GROUP_ID] is dragged_pose + assert gui.state.group_poses[DEFAULT_GROUP_ID] is solved_pose + + +def test_gui_can_disable_collision_check_for_cartesian_target_evaluation( + make_panel: Callable[..., ViserPanelGui], +) -> None: + current = FakeJointState(["j1", "j2"], position=[0.0, 0.0]) + config = make_robot_config(joint_names=["j1", "j2"], home_joints=[0.0, 0.0]) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: current, + is_state_stale=lambda robot_id, max_age=1.0: False, + is_state_valid=lambda robot_id, joint_state: False, + get_ee_pose=lambda robot_id, joint_state=None: Pose( + {"position": [0.0, 0.0, 0.0], "orientation": [0.0, 0.0, 0.0, 1.0]} + ), + ) + adapter = InProcessViserAdapter(world_monitor=world_monitor, manipulation_module=module) + scene = SimpleNamespace( + has_reference_grid=lambda: False, + ensure_target_controls=lambda *args: None, + set_target_joints=lambda *args: True, + set_target_pose=lambda *args: None, + set_target_visual_state=lambda *args: None, + ) + gui = make_panel( + FakeGuiServer(), + adapter, + ViserVisualizationConfig(panel_enabled=True), + scene, + ) + request = TargetEvaluationRequest( + sequence_id=1, + source="cartesian", + group_ids=(DEFAULT_GROUP_ID,), + pose_targets={ + DEFAULT_GROUP_ID: Pose( + {"position": [0.1, 0.2, 0.3], "orientation": [0.0, 0.0, 0.0, 1.0]} + ) + }, + check_collision=False, + ) + + result = gui._handle_target_evaluation_request(request) + + assert result["success"] is True + assert result["collision_free"] is True + assert result["message"] == "Target collision check skipped" def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( @@ -1354,9 +1778,7 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( ) -> None: current = FakeJointState(["j1"], position=[0.0]) config = make_robot_config(joint_names=["j1"], home_joints=[0.0]) - module = FakeManipulationModule( - _robots={"arm": ("robot-1", config, None)}, _planned_paths={}, _planned_trajectories={} - ) + module = FakeManipulationModule(_robots={"arm": ("robot-1", config, None)}) world_monitor = SimpleNamespace( get_current_joint_state=lambda robot_id: current, is_state_stale=lambda robot_id, max_age=1.0: False, @@ -1376,7 +1798,7 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( set_target_visual_state=lambda *args: visual_states.append(args), ) gui = make_panel(FakeGuiServer(), adapter, ViserVisualizationConfig(panel_enabled=True), scene) - request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") + request = TargetEvaluationRequest(sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,)) gui.state.latest_sequence_id = 1 result = adapter.evaluate_joint_target(FakeJointState(["j1"], position=[1.0]), "arm") @@ -1386,7 +1808,7 @@ def test_gui_collision_evaluation_marks_target_infeasible_and_colors_scene( assert gui.state.target_status == TargetStatus.INFEASIBLE assert gui.state.feasibility.status == FeasibilityStatus.COLLISION assert gui.state.error == "Target is in collision" - assert visual_states[-1] == ("robot-1", False) + assert visual_states[-1] == (DEFAULT_GROUP_ID, False) def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( @@ -1400,8 +1822,6 @@ def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( _robots={ "arm": ("robot-1", make_robot_config(joint_names=["j1"], home_joints=[1.0]), None) }, - _planned_paths={"arm": planned}, - _planned_trajectories={}, _state=NamedState(name="IDLE"), execute=lambda robot_name=None: executed.append(robot_name) or True, clear_planned_path=lambda: cleared.append(True) or True, @@ -1433,8 +1853,8 @@ def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( gui.state.target_status = TargetStatus.FEASIBLE gui.state.plan_state = PanelPlanState( status=PlanStatus.FRESH, - robot="arm", - start_joints_snapshot=[1.2], + group_ids=(DEFAULT_GROUP_ID,), + start_joints_snapshot={DEFAULT_GROUP_ID: [1.2]}, planned_path=planned, ) gui._submit_execute() @@ -1443,9 +1863,9 @@ def test_gui_safe_execute_requires_fresh_matching_plan_and_clear_resets_path( gui.state.action_status = ActionStatus.IDLE gui.state.error = "" - gui.state.plan_state.start_joints_snapshot = [1.0] + gui.state.plan_state.start_joints_snapshot = {DEFAULT_GROUP_ID: [1.0]} gui._submit_execute() - assert executed == ["arm"] + assert executed == [None] gui._submit_clear() assert cleared == [True] @@ -1466,7 +1886,8 @@ def test_gui_plan_target_failure_recovers_action_state( submit=lambda operation, **_kwargs: operation(), stop=lambda timeout=2.0: None ), ) - gui.state.selected_robot = "missing" + gui.state.selected_group_ids = ("missing",) + gui.state.target_joints = JointState({"name": ["missing/j1"], "position": [1.0]}) gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "IDLE" @@ -1474,7 +1895,7 @@ def test_gui_plan_target_failure_recovers_action_state( assert gui.state.action_status == ActionStatus.IDLE assert gui.state.plan_state.status == PlanStatus.FAILED - assert gui.state.error == "No robot config" + assert gui.state.error == "Unknown planning group: missing" assert gui.state.last_result == "plan_to_joints=False" @@ -1498,12 +1919,12 @@ def reset() -> bool: calls.append("reset") return True - def plan_to_joints(_joints: JointState, _robot_name: str | None = None) -> bool: + def plan_target_set(_joint_targets: dict[str, JointState]) -> bool: calls.append("plan") return True monkeypatch.setattr(adapter, "reset", reset) - monkeypatch.setattr(adapter, "plan_to_joints", plan_to_joints) + monkeypatch.setattr(adapter, "plan_target_set", plan_target_set) gui.state.target_status = TargetStatus.FEASIBLE gui.state.manipulation_state = "FAULT" @@ -1562,8 +1983,12 @@ def operation() -> None: def test_target_evaluation_worker_coalesces_pending_requests() -> None: worker = TargetEvaluationWorker(lambda request: {}, lambda request, result: None) - old_request = TargetEvaluationRequest(sequence_id=1, source="joints", robot_name="arm") - new_request = TargetEvaluationRequest(sequence_id=2, source="joints", robot_name="arm") + old_request = TargetEvaluationRequest( + sequence_id=1, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) + new_request = TargetEvaluationRequest( + sequence_id=2, source="joints", group_ids=(DEFAULT_GROUP_ID,) + ) worker.submit(old_request) worker.submit(new_request) diff --git a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py index 19f1f2da60..bb7ffd8d47 100644 --- a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py +++ b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py @@ -22,12 +22,14 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.manipulation.planning.spec.models import PlanningSceneInfo +from dimos.manipulation.planning.spec.enums import PlanningStatus +from dimos.manipulation.planning.spec.models import GeneratedPlan, PlanningSceneInfo from dimos.manipulation.visualization.viser import ( runtime as runtime_module, visualizer as visualizer_module, ) from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter +from dimos.manipulation.visualization.viser.animation import GroupPreviewAnimation from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.runtime import ViserRuntime from dimos.manipulation.visualization.viser.visualizer import ViserManipulationVisualizer @@ -62,7 +64,7 @@ def fake_robot_config(name: str) -> RobotModelConfig: name=name, model_path=Path(f"{name}.urdf"), base_pose=PoseStamped(), - joint_names=[], + joint_names=["joint1"], end_effector_link="ee_link", ) @@ -407,18 +409,30 @@ def show_preview(self, robot_id: str) -> None: def hide_preview(self, robot_id: str) -> None: calls.append(("hide", robot_id)) - def animate_path(self, robot_id: str, path: list[JointState], duration: float) -> None: - assert path == [current] + def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> None: + assert preview.group_ids == ("arm/manipulator",) + assert len(preview.tracks) == 1 + assert preview.tracks[0].path == (current,) assert duration == 1.5 - calls.append(("animate", robot_id)) + calls.append(("animate", preview.tracks[0].robot_id)) def close(self) -> None: calls.append(("scene", "close")) - world_monitor = SimpleNamespace(get_current_joint_state=lambda _robot_id: current) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda _robot_id: current, + planning_groups=SimpleNamespace( + select=lambda _group_ids: SimpleNamespace( + groups=(SimpleNamespace(id="arm/manipulator", robot_name="arm"),), + robot_names=("arm",), + ) + ), + ) + robot_config = fake_robot_config("arm") manipulation_module = SimpleNamespace( - robot_items=lambda: [("arm", "robot-1", fake_robot_config("arm"))], + robot_items=lambda: [("arm", "robot-1", robot_config)], robot_id_for_name=lambda robot_name: "robot-1" if robot_name == "arm" else None, + get_robot_config=lambda robot_name: robot_config if robot_name == "arm" else None, ) monkeypatch.setattr(visualizer_module, "ViserRuntime", FakeRuntime) monkeypatch.setattr(visualizer_module, "ViserUrdf", FakeViserUrdf) @@ -430,9 +444,14 @@ def close(self) -> None: ) visualizer.publish_visualization() - visualizer.show_preview("robot-1") - visualizer.hide_preview("robot-1") - visualizer.animate_path("robot-1", [current], duration=1.5) + visualizer.show_preview(("arm/manipulator",)) + visualizer.hide_preview(("arm/manipulator",)) + plan = GeneratedPlan( + group_ids=("arm/manipulator",), + path=[JointState(name=["arm/joint1"], position=[0.5])], + status=PlanningStatus.SUCCESS, + ) + visualizer.animate_plan(plan, duration=1.5) visualizer.close() visualizer.publish_visualization() @@ -446,3 +465,86 @@ def close(self) -> None: ("scene", "close"), ("runtime", "close"), ] + + +def test_visualizer_animates_multi_robot_plan_as_one_group_preview( + monkeypatch: pytest.MonkeyPatch, +) -> None: + previews: list[GroupPreviewAnimation] = [] + + class FakeRuntime: + url = "http://localhost:8095" + + def __init__(self, config: ViserVisualizationConfig) -> None: + self.config = config + + def start(self) -> FakeServer: + return FakeServer() + + def close(self) -> None: + pass + + class FakeScene: + def __init__( + self, + server: FakeServer, + viser_urdf: type[FakeViserUrdf], + *, + preview_fps: float, + ) -> None: + pass + + def animate_preview(self, preview: GroupPreviewAnimation, duration: float) -> None: + assert duration == 2.0 + previews.append(preview) + + def close(self) -> None: + pass + + groups = ( + SimpleNamespace(id="left/arm", robot_name="left"), + SimpleNamespace(id="right/arm", robot_name="right"), + ) + world_monitor = SimpleNamespace( + get_current_joint_state=lambda robot_id: JointState( + {"name": ["joint1"], "position": [0.0 if robot_id == "left-id" else 10.0]} + ), + planning_groups=SimpleNamespace( + select=lambda _group_ids: SimpleNamespace(groups=groups, robot_names=("left", "right")) + ), + ) + configs = {"left": fake_robot_config("left"), "right": fake_robot_config("right")} + manipulation_module = SimpleNamespace( + robot_id_for_name=lambda robot_name: f"{robot_name}-id" if robot_name in configs else None, + get_robot_config=lambda robot_name: configs.get(robot_name), + ) + monkeypatch.setattr(visualizer_module, "ViserRuntime", FakeRuntime) + monkeypatch.setattr(visualizer_module, "ViserUrdf", FakeViserUrdf) + monkeypatch.setattr(visualizer_module, "ViserManipulationScene", FakeScene) + visualizer = ViserManipulationVisualizer( + world_monitor=world_monitor, + manipulation_module=manipulation_module, + config=ViserVisualizationConfig(panel_enabled=False), + ) + + visualizer.animate_plan( + GeneratedPlan( + group_ids=("left/arm", "right/arm"), + path=[ + JointState(name=["left/joint1", "right/joint1"], position=[0.0, 10.0]), + JointState(name=["left/joint1", "right/joint1"], position=[1.0, 11.0]), + ], + status=PlanningStatus.SUCCESS, + ), + duration=2.0, + ) + + assert len(previews) == 1 + preview = previews[0] + assert preview.group_ids == ("left/arm", "right/arm") + assert [(track.robot_id, track.group_ids) for track in preview.tracks] == [ + ("left-id", ("left/arm",)), + ("right-id", ("right/arm",)), + ] + assert [tuple(point.position) for point in preview.tracks[0].path] == [(0.0,), (1.0,)] + assert [tuple(point.position) for point in preview.tracks[1].path] == [(10.0,), (11.0,)] diff --git a/dimos/manipulation/visualization/viser/theme.py b/dimos/manipulation/visualization/viser/theme.py index 339be04492..d38767d1da 100644 --- a/dimos/manipulation/visualization/viser/theme.py +++ b/dimos/manipulation/visualization/viser/theme.py @@ -36,7 +36,7 @@ DIMOS_THEME_TITLE = "DimOS Manipulation" DIMOS_THEME_URL = "https://github.com/dimensionalOS/dimos" -DIMOS_BRAND_COLOR = (22, 130, 163) +DIMOS_BRAND_COLOR = (0, 153, 255) DIMOS_LOGO_PATH = Path(__file__).with_name("assets") / "dimensional-logo.svg" logger = setup_logger() @@ -86,8 +86,8 @@ def _configure_theme(server: ViserServer, titlebar_content: TitlebarConfig | Non try: server.gui.configure_theme( titlebar_content=titlebar_content, - control_layout="collapsible", - control_width="medium", + control_layout="fixed", + control_width="large", dark_mode=True, show_logo=False, show_share_button=False, diff --git a/dimos/manipulation/visualization/viser/visualizer.py b/dimos/manipulation/visualization/viser/visualizer.py index d12c98e53b..0ea7fb575b 100644 --- a/dimos/manipulation/visualization/viser/visualizer.py +++ b/dimos/manipulation/visualization/viser/visualizer.py @@ -14,10 +14,16 @@ from __future__ import annotations +from collections.abc import Sequence from contextlib import suppress from typing import TYPE_CHECKING +from dimos.manipulation.planning.planning_identifiers import make_global_joint_name from dimos.manipulation.visualization.viser.adapter import InProcessViserAdapter +from dimos.manipulation.visualization.viser.animation import ( + GroupPreviewAnimation, + PreviewTrack, +) from dimos.manipulation.visualization.viser.config import ViserVisualizationConfig from dimos.manipulation.visualization.viser.gui import ViserPanelGui from dimos.manipulation.visualization.viser.runtime import ( @@ -27,6 +33,7 @@ ) from dimos.manipulation.visualization.viser.scene import ViserManipulationScene from dimos.manipulation.visualization.viser.theme import apply_dimos_theme +from dimos.msgs.sensor_msgs.JointState import JointState from dimos.utils.logging_config import setup_logger try: @@ -43,10 +50,11 @@ if TYPE_CHECKING: from dimos.manipulation.manipulation_module import ManipulationModule from dimos.manipulation.planning.monitor.world_monitor import WorldMonitor + from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( - JointPath, + GeneratedPlan, + PlanningGroupID, PlanningSceneInfo, - WorldRobotID, ) logger = setup_logger() @@ -153,32 +161,120 @@ def publish_visualization(self, ctx: None = None) -> None: if self._gui is not None: self._gui.refresh() - def show_preview(self, robot_id: WorldRobotID) -> None: + def show_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: if not self._closed: self._ensure_started() if self._scene is None: return - self._scene.show_preview(str(robot_id)) + for robot_id in self._robot_ids_for_groups(group_ids): + self._scene.show_preview(str(robot_id)) - def hide_preview(self, robot_id: WorldRobotID) -> None: + def hide_preview(self, group_ids: Sequence[PlanningGroupID]) -> None: if not self._closed: self._ensure_started() if self._scene is None: return - self._scene.hide_preview(str(robot_id)) + for robot_id in self._robot_ids_for_groups(group_ids): + self._scene.hide_preview(str(robot_id)) - def animate_path( - self, - robot_id: WorldRobotID, - path: JointPath, - duration: float = 3.0, - ) -> None: + def animate_plan(self, plan: GeneratedPlan, duration: float = 3.0) -> None: if self._closed: return self._ensure_started() - if self._scene is None: + if self._adapter is None or self._scene is None: return - self._scene.animate_path(str(robot_id), list(path), duration) + preview = self._build_group_preview_animation(plan) + if preview is not None: + self._scene.animate_preview(preview, duration) + + def _build_group_preview_animation(self, plan: GeneratedPlan) -> GroupPreviewAnimation | None: + if self._adapter is None: + return None + selection = self._world_monitor.planning_groups.select(plan.group_ids) + tracks: list[PreviewTrack] = [] + for robot_name in selection.robot_names: + robot_id = self._adapter.robot_id_for_name(robot_name) + config = self._adapter.get_robot_config(robot_name) + current = self._adapter.get_current_joint_state(robot_name) + if robot_id is None or config is None or current is None: + logger.warning( + "Cannot build group preview for robot '%s': missing id, config, or state", + robot_name, + ) + return None + path = self._robot_path_for_plan(robot_name, config, current, plan) + if not path: + logger.warning("Cannot project generated plan for robot '%s'", robot_name) + return None + tracks.append( + PreviewTrack( + robot_id=str(robot_id), + group_ids=tuple( + group.id for group in selection.groups if group.robot_name == robot_name + ), + joint_names=tuple(config.joint_names), + path=tuple(path), + ) + ) + if not tracks: + return None + return GroupPreviewAnimation(group_ids=plan.group_ids, tracks=tuple(tracks)) + + def _robot_ids_for_groups(self, group_ids: Sequence[PlanningGroupID]) -> list[str]: + if self._adapter is None: + return [] + selection = self._world_monitor.planning_groups.select(group_ids) + robot_ids: list[str] = [] + for robot_name in selection.robot_names: + robot_id = self._adapter.robot_id_for_name(robot_name) + if robot_id is not None: + robot_ids.append(str(robot_id)) + return robot_ids + + def _robot_path_for_plan( + self, + robot_name: str, + config: RobotModelConfig, + current: JointState, + plan: GeneratedPlan, + ) -> list[JointState]: + current_by_name = self._current_positions_by_name(config, current) + if current_by_name is None: + return [] + path: list[JointState] = [] + for waypoint in plan.path: + if len(waypoint.name) != len(waypoint.position): + return [] + selected = dict(zip(waypoint.name, waypoint.position, strict=True)) + positions: list[float] = [] + for local_name in config.joint_names: + global_name = make_global_joint_name(robot_name, local_name) + if global_name in selected: + positions.append(float(selected[global_name])) + continue + if local_name not in current_by_name: + return [] + positions.append(current_by_name[local_name]) + path.append(JointState(name=list(config.joint_names), position=positions)) + return path + + @staticmethod + def _current_positions_by_name( + config: RobotModelConfig, current: JointState + ) -> dict[str, float] | None: + if current.name: + if len(current.name) != len(current.position): + return None + return { + str(name): float(position) + for name, position in zip(current.name, current.position, strict=True) + } + if len(current.position) != len(config.joint_names): + return None + return { + str(name): float(position) + for name, position in zip(config.joint_names, current.position, strict=True) + } def close(self) -> None: if self._closed: diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 291af8540e..cc5797553d 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -58,7 +58,8 @@ "desk-marker-tf": "dimos.perception.fiducial.blueprints.desk_marker_tf:desk_marker_tf", "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", - "dual-xarm6-planner": "dimos.manipulation.blueprints:dual_xarm6_planner", + "dual-xarm6-planner-coordinator": "dimos.manipulation.blueprints:dual_xarm6_planner_coordinator", + "dual-xarm7-planner-coordinator": "dimos.manipulation.blueprints:dual_xarm7_planner_coordinator", "keyboard-teleop-a750": "dimos.robot.manipulators.a750.blueprints:keyboard_teleop_a750", "keyboard-teleop-openarm": "dimos.robot.manipulators.openarm.blueprints:keyboard_teleop_openarm", "keyboard-teleop-openarm-mock": "dimos.robot.manipulators.openarm.blueprints:keyboard_teleop_openarm_mock", diff --git a/dimos/robot/catalog/openarm.py b/dimos/robot/catalog/openarm.py new file mode 100644 index 0000000000..fb57e013b5 --- /dev/null +++ b/dimos/robot/catalog/openarm.py @@ -0,0 +1,116 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""OpenArm v10 robot configurations.""" + +from __future__ import annotations + +from typing import Any + +from dimos.robot.config import RobotConfig +from dimos.utils.data import LfsPath + +# Collision exclusion pairs — structural mesh overlaps in the OpenArm URDF. +# link5 and link7 collision meshes overlap by ~3mm at zero pose (and every +# other pose) — same pattern as R1 Pro's non-adjacent link overlap. +OPENARM_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ + ("openarm_left_link5", "openarm_left_link7"), + ("openarm_right_link5", "openarm_right_link7"), +] + +# LFS-backed: data/.lfs/openarm_description.tar.gz extracts to data/openarm_description/ +_OPENARM_PKG = LfsPath("openarm_description") +_OPENARM_MODEL_PATH = _OPENARM_PKG / "urdf/robot/openarm_v10_bimanual.urdf" +# Per-side URDFs: extracted from bimanual expansion, only one arm + torso each. +# Avoids phantom-arm collisions when Drake loads both sides into one world. +_OPENARM_LEFT_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_left.urdf" +_OPENARM_RIGHT_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_right.urdf" + +# Pre-expanded single-arm URDF for Pinocchio FK (keyboard teleop, IK, etc.) +OPENARM_V10_FK_MODEL = _OPENARM_PKG / "urdf/robot/openarm_v10_single.urdf" + + +def openarm_arm( + side: str = "left", + name: str | None = None, + *, + adapter_type: str = "mock", + address: str | None = None, + **overrides: Any, +) -> RobotConfig: + """OpenArm v10 config for one side. Uses per-side URDF (arm + torso only).""" + if side not in ("left", "right"): + raise ValueError(f"side must be 'left' or 'right', got {side!r}") + + resolved_name = name or f"{side}_arm" + # Pre-expanded bimanual URDF uses openarm_{side}_* naming. + joint_names = [f"openarm_{side}_joint{i}" for i in range(1, 8)] + ee_link = f"openarm_{side}_link7" + + defaults: dict[str, Any] = { + "name": resolved_name, + "model_path": _OPENARM_LEFT_MODEL if side == "left" else _OPENARM_RIGHT_MODEL, + "end_effector_link": ee_link, + "adapter_type": adapter_type, + "address": address, + "joint_names": joint_names, + "base_link": "openarm_body_link0", + "home_joints": [0.0] * 7, + "base_pose": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + "package_paths": {"openarm_description": _OPENARM_PKG}, + "collision_exclusion_pairs": OPENARM_COLLISION_EXCLUSIONS, + "auto_convert_meshes": True, + "max_velocity": 0.5, + "max_acceleration": 1.0, + "adapter_kwargs": {"side": side}, + } + # Merge adapter_kwargs rather than replace, so callers can add keys + # (e.g. auto_set_mit_mode) without clobbering the catalog's "side". + if "adapter_kwargs" in overrides: + defaults["adapter_kwargs"] = { + **defaults["adapter_kwargs"], + **overrides.pop("adapter_kwargs"), + } + defaults.update(overrides) + return RobotConfig(**defaults) + + +def openarm_single( + name: str = "arm", + *, + adapter_type: str = "mock", + address: str | None = None, + **overrides: Any, +) -> RobotConfig: + """Single-arm config (keyboard teleop, cartesian IK). Use openarm_arm() for bimanual.""" + defaults: dict[str, Any] = { + "name": name, + "model_path": OPENARM_V10_FK_MODEL, + "end_effector_link": "openarm_left_link7", + "adapter_type": adapter_type, + "address": address, + "joint_names": [f"openarm_left_joint{i}" for i in range(1, 8)], + "base_link": "openarm_body_link0", + "home_joints": [0.0] * 7, + "package_paths": {"openarm_description": _OPENARM_PKG}, + "auto_convert_meshes": True, + "max_velocity": 0.5, + "max_acceleration": 1.0, + "adapter_kwargs": {"side": "left"}, + } + defaults.update(overrides) + return RobotConfig(**defaults) + + +__all__ = ["OPENARM_V10_FK_MODEL", "openarm_arm", "openarm_single"] diff --git a/dimos/robot/catalog/test_ufactory.py b/dimos/robot/catalog/test_ufactory.py new file mode 100644 index 0000000000..3875ff9c92 --- /dev/null +++ b/dimos/robot/catalog/test_ufactory.py @@ -0,0 +1,40 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from __future__ import annotations + +from collections.abc import Callable +import math + +import pytest + +from dimos.robot.catalog.ufactory import xarm6, xarm7 +from dimos.robot.config import RobotConfig + + +@pytest.mark.parametrize("factory", [xarm6, xarm7]) +def test_xarm_instance_offsets_are_encoded_only_in_base_pose( + factory: Callable[..., RobotConfig], +) -> None: + config = factory(name="arm", y_offset=0.5, pitch=0.25) + model_config = config.to_robot_model_config() + + assert model_config.xacro_args["attach_xyz"] == "0 0 0" + assert model_config.xacro_args["attach_rpy"] == "0 0 0" + assert model_config.base_pose.position.y == 0.5 + assert model_config.base_pose.orientation.x == 0.0 + assert model_config.base_pose.orientation.y == pytest.approx(math.sin(0.125)) + assert model_config.base_pose.orientation.z == 0.0 + assert model_config.base_pose.orientation.w == pytest.approx(math.cos(0.125)) + assert model_config.strip_model_world_joint is True diff --git a/dimos/robot/catalog/ufactory.py b/dimos/robot/catalog/ufactory.py new file mode 100644 index 0000000000..9c21585d2a --- /dev/null +++ b/dimos/robot/catalog/ufactory.py @@ -0,0 +1,174 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""UFactory xArm robot configurations.""" + +from __future__ import annotations + +import math +from typing import Any + +from dimos.robot.config import GripperConfig, RobotConfig +from dimos.utils.data import LfsPath + +# Pre-built URDFs for Pinocchio FK (xacro not supported by Pinocchio) +XARM6_FK_MODEL = LfsPath("xarm_description/urdf/xarm6/xarm6.urdf") +XARM7_FK_MODEL = LfsPath("xarm_description/urdf/xarm7/xarm7.urdf") + +# Simulation model paths (MJCF) +XARM7_SIM_PATH = LfsPath("xarm7/scene.xml") +XARM6_SIM_PATH = LfsPath("xarm6/scene.xml") + +# XArm gripper collision exclusions (parallel linkage mechanism) +# The gripper uses mimic joints where non-adjacent links can overlap legitimately +XARM_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ + ("right_inner_knuckle", "right_outer_knuckle"), + ("left_inner_knuckle", "left_outer_knuckle"), + ("right_inner_knuckle", "right_finger"), + ("left_inner_knuckle", "left_finger"), + ("left_finger", "right_finger"), + ("left_outer_knuckle", "right_outer_knuckle"), + ("left_inner_knuckle", "right_inner_knuckle"), + ("left_outer_knuckle", "right_finger"), + ("right_outer_knuckle", "left_finger"), + ("xarm_gripper_base_link", "left_inner_knuckle"), + ("xarm_gripper_base_link", "right_inner_knuckle"), + ("xarm_gripper_base_link", "left_finger"), + ("xarm_gripper_base_link", "right_finger"), + ("link6", "xarm_gripper_base_link"), + ("link6", "left_outer_knuckle"), + ("link6", "right_outer_knuckle"), +] + + +def _base_pose_from_offsets( + x_offset: float, y_offset: float, z_offset: float, pitch: float +) -> list[float]: + half_pitch = pitch / 2.0 + return [x_offset, y_offset, z_offset, 0.0, math.sin(half_pitch), 0.0, math.cos(half_pitch)] + + +def xarm7( + name: str = "arm", + *, + adapter_type: str = "mock", + address: str | None = None, + add_gripper: bool = False, + x_offset: float = 0.0, + y_offset: float = 0.0, + z_offset: float = 0.0, + pitch: float = 0.0, + tf_extra_links: list[str] | None = None, + **overrides: Any, +) -> RobotConfig: + """Create an xArm7 robot configuration.""" + xacro_args: dict[str, str] = { + "dof": "7", + "limited": "true", + "attach_xyz": "0 0 0", + "attach_rpy": "0 0 0", + } + if add_gripper: + xacro_args["add_gripper"] = "true" + + defaults: dict[str, Any] = { + "name": name, + "model_path": LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro", + "end_effector_link": "link_tcp" if add_gripper else "link7", + "adapter_type": adapter_type, + "address": address, + "joint_names": [f"joint{i}" for i in range(1, 8)], + "base_link": "link_base", + "home_joints": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + "base_pose": _base_pose_from_offsets(x_offset, y_offset, z_offset, pitch), + "strip_model_world_joint": True, + "package_paths": {"xarm_description": LfsPath("xarm_description")}, + "xacro_args": xacro_args, + "auto_convert_meshes": True, + "collision_exclusion_pairs": XARM_GRIPPER_COLLISION_EXCLUSIONS if add_gripper else [], + "tf_extra_links": tf_extra_links or [], + "gripper": GripperConfig( + type="xarm", + joints=["gripper"], + collision_exclusions=XARM_GRIPPER_COLLISION_EXCLUSIONS, + open_position=0.85, + close_position=0.0, + ) + if add_gripper + else None, + } + defaults.update(overrides) + return RobotConfig(**defaults) + + +def xarm6( + name: str = "arm", + *, + adapter_type: str = "mock", + address: str | None = None, + add_gripper: bool = True, + x_offset: float = 0.0, + y_offset: float = 0.0, + z_offset: float = 0.0, + pitch: float = 0.0, + tf_extra_links: list[str] | None = None, + **overrides: Any, +) -> RobotConfig: + """Create an xArm6 robot configuration.""" + xacro_args: dict[str, str] = { + "dof": "6", + "limited": "true", + "attach_xyz": "0 0 0", + "attach_rpy": "0 0 0", + } + if add_gripper: + xacro_args["add_gripper"] = "true" + + defaults: dict[str, Any] = { + "name": name, + "model_path": LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro", + "end_effector_link": "link_tcp" if add_gripper else "link6", + "adapter_type": adapter_type, + "address": address, + "joint_names": [f"joint{i}" for i in range(1, 7)], + "base_link": "link_base", + "home_joints": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + "base_pose": _base_pose_from_offsets(x_offset, y_offset, z_offset, pitch), + "strip_model_world_joint": True, + "package_paths": {"xarm_description": LfsPath("xarm_description")}, + "xacro_args": xacro_args, + "auto_convert_meshes": True, + "collision_exclusion_pairs": XARM_GRIPPER_COLLISION_EXCLUSIONS if add_gripper else [], + "tf_extra_links": tf_extra_links or [], + "gripper": GripperConfig( + type="xarm", + joints=["gripper"], + collision_exclusions=XARM_GRIPPER_COLLISION_EXCLUSIONS, + open_position=0.85, + close_position=0.0, + ) + if add_gripper + else None, + } + defaults.update(overrides) + return RobotConfig(**defaults) + + +__all__ = [ + "XARM6_FK_MODEL", + "XARM7_FK_MODEL", + "XARM_GRIPPER_COLLISION_EXCLUSIONS", + "xarm6", + "xarm7", +] diff --git a/dimos/robot/config.py b/dimos/robot/config.py new file mode 100644 index 0000000000..13a3aa83ee --- /dev/null +++ b/dimos/robot/config.py @@ -0,0 +1,293 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Unified robot configuration. + +Single source of truth for a robot. The URDF/MJCF model file is the +ground truth — joint names, DOF, limits, and link hierarchy are parsed +automatically. Generates RobotModelConfig, HardwareComponent, and TaskConfig. +""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +from pydantic import BaseModel, Field, PrivateAttr + +from dimos.control.components import HardwareComponent, HardwareType +from dimos.control.coordinator import TaskConfig +from dimos.manipulation.planning.groups.discovery import discover_planning_group_definitions +from dimos.manipulation.planning.planning_identifiers import make_global_joint_names +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.robot.model_parser import ModelDescription, parse_model + + +class GripperConfig(BaseModel): + """Gripper configuration.""" + + type: str + joints: list[str] = Field(default_factory=list) + collision_exclusions: list[tuple[str, str]] = Field(default_factory=list) + open_position: float = 1.0 + close_position: float = 0.0 + + +class RobotConfig(BaseModel): + """Unified robot configuration — URDF/MJCF is the ground truth. + + Model parsing is lazy to avoid LFS downloads at import time. + """ + + # Required fields + name: str + model_path: Path | None = None + # Compatibility robot-scoped target frame; new planning uses group tip links. + end_effector_link: str | None = None + + # Physical dimensions (meters) + height_clearance: float | None = None # max height + width_clearance: float | None = None # max width + + # These offsets are applied so that odometry at 0,0,0 corresponds roughly with the floor + # Note: these cannot (easily) be calculated from the URDF because + # the URDF doesn't always have an initial robot pose/stance + # This is a quality of life offset, not exact + # The key names should match keys in the urdf + internal_odom_offsets: dict[str, Any] = Field(default_factory=dict) + + # Hardware connection + adapter_type: str = "mock" + address: str | None = None + adapter_kwargs: dict[str, Any] = Field(default_factory=dict) + auto_enable: bool = True + + # Optional overrides (derived from model if not set) + joint_names: list[str] | None = None + base_link: str | None = ( + None # Compatibility planning override; derived from model root when absent. + ) + home_joints: list[float] | None = None + + # Canonical planning placement. Robot models should describe intrinsic geometry; + # instance placement belongs here. + base_pose: list[float] = Field(default_factory=lambda: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) + strip_model_world_joint: bool = False + + # Planning + max_velocity: float = 1.0 + max_acceleration: float = 2.0 + pre_grasp_offset: float = 0.10 + + # Gripper + gripper: GripperConfig | None = None + + # Model loading + package_paths: dict[str, Path] = Field(default_factory=dict) + xacro_args: dict[str, str] = Field(default_factory=dict) + auto_convert_meshes: bool = True + srdf_path: Path | None = None + + # TF publishing + tf_extra_links: list[str] = Field(default_factory=list) + + # Task defaults + task_type: str = "trajectory" + task_priority: int = 10 + + # Collision exclusion pairs (gripper-specific, cannot be parsed from model) + collision_exclusion_pairs: list[tuple[str, str]] = Field(default_factory=list) + + _parsed: ModelDescription | None = PrivateAttr(default=None) + + def _ensure_parsed(self) -> ModelDescription: + """Parse model lazily on first access.""" + if self._parsed is None: + if self.model_path is None: + raise ValueError( + f"RobotConfig '{self.name}' has no model_path — " + "joint/link info is unavailable. Set model_path to a URDF/MJCF." + ) + self._parsed = parse_model(self.model_path, self.package_paths, self.xacro_args) + if self.joint_names is None: + self.joint_names = self._parsed.actuated_joint_names + if self.base_link is None: + self.base_link = self._parsed.root_link + if self.home_joints is None: + self.home_joints = self._compute_default_home() + return self._parsed + + def _compute_default_home(self) -> list[float]: + assert self._parsed is not None + home = [] + for joint_name in self.local_joint_names: + joint = self._parsed.get_joint(joint_name) + if ( + joint is not None + and joint.lower_limit is not None + and joint.upper_limit is not None + ): + home.append((joint.lower_limit + joint.upper_limit) / 2.0) + else: + home.append(0.0) + return home + + # -- Derived properties --------------------------------------------------- + + @property + def model_description(self) -> ModelDescription: + return self._ensure_parsed() + + @property + def local_joint_names(self) -> list[str]: + self._ensure_parsed() + assert self.joint_names is not None + return self.joint_names + + @property + def global_joint_names(self) -> list[str]: + return make_global_joint_names(self.name, self.local_joint_names) + + @property + def resolved_base_link(self) -> str: + self._ensure_parsed() + assert self.base_link is not None + return self.base_link + + @property + def dof(self) -> int: + if self.joint_names is not None: + return len(self.joint_names) + return len(self.local_joint_names) + + @property + def coordinator_task_name(self) -> str: + return f"traj_{self.name}" + + # -- Converter methods ---------------------------------------------------- + + def to_robot_model_config(self) -> RobotModelConfig: + """Generate RobotModelConfig for ManipulationModule.""" + if self.model_path is None: + raise ValueError( + f"RobotConfig '{self.name}' has no model_path — " + "cannot generate RobotModelConfig for manipulation." + ) + bp = self.base_pose + base_pose = PoseStamped( + position=Vector3(x=bp[0], y=bp[1], z=bp[2]), + orientation=Quaternion(bp[3], bp[4], bp[5], bp[6]), + ) + + exclusions = list(self.collision_exclusion_pairs) + if self.gripper: + exclusions.extend(self.gripper.collision_exclusions) + + # Use direct fields when available to avoid triggering model parsing at import time + joint_names = self.joint_names if self.joint_names is not None else self.local_joint_names + base_link = self.base_link if self.base_link is not None else self.resolved_base_link + planning_groups = discover_planning_group_definitions( + robot_name=self.name, + model_path=self.model_path, + model=self.model_description, + controllable_joint_names=joint_names, + srdf_path=self.srdf_path, + ) + legacy_end_effector_link = self.end_effector_link or next( + (group.tip_link for group in planning_groups if group.tip_link is not None), + None, + ) + + return RobotModelConfig( + name=self.name, + model_path=self.model_path, + srdf_path=self.srdf_path, + base_pose=base_pose, + strip_model_world_joint=self.strip_model_world_joint, + joint_names=joint_names, + end_effector_link=legacy_end_effector_link, + base_link=base_link, + planning_groups=planning_groups, + package_paths=self.package_paths, + xacro_args=self.xacro_args, + collision_exclusion_pairs=exclusions, + auto_convert_meshes=self.auto_convert_meshes, + max_velocity=self.max_velocity, + max_acceleration=self.max_acceleration, + coordinator_task_name=self.coordinator_task_name, + gripper_hardware_id=self.name if self.gripper else None, + tf_extra_links=self.tf_extra_links, + home_joints=self.home_joints, + pre_grasp_offset=self.pre_grasp_offset, + ) + + def to_hardware_component(self) -> HardwareComponent: + """Generate HardwareComponent for ControlCoordinator.""" + gripper_joints: list[str] = [] + if self.gripper and self.gripper.joints: + gripper_joints = make_global_joint_names(self.name, self.gripper.joints) + + adapter_kwargs = dict(self.adapter_kwargs) + if self.home_joints is not None: + adapter_kwargs.setdefault("initial_positions", self.home_joints) + + return HardwareComponent( + hardware_id=self.name, + hardware_type=HardwareType.MANIPULATOR, + joints=self.global_joint_names, + adapter_type=self.adapter_type, + address=self.address, + auto_enable=self.auto_enable, + gripper_joints=gripper_joints, + adapter_kwargs=adapter_kwargs, + ) + + def to_task_config( + self, + task_type: str | None = None, + task_name: str | None = None, + priority: int | None = None, + auto_start: bool = False, + **task_kwargs: Any, + ) -> TaskConfig: + """Generate TaskConfig for ControlCoordinator. + + Args: + task_type: Override task type (default: self.task_type). + task_name: Override task name (default: self.coordinator_task_name). + priority: Override priority (default: self.task_priority). + auto_start: Whether the coordinator should start this task on startup. + **task_kwargs: Task-specific params (e.g., model_path, + ee_joint_id, hand, gripper_joint, gripper_open_pos, gripper_closed_pos). + """ + params = dict(task_kwargs.pop("params", {})) + params.update(task_kwargs) + + return TaskConfig( + name=task_name if task_name is not None else self.coordinator_task_name, + type=task_type if task_type is not None else self.task_type, + joint_names=self.global_joint_names, + priority=priority if priority is not None else self.task_priority, + auto_start=auto_start, + params=params, + ) + + +__all__ = [ + "GripperConfig", + "RobotConfig", +] diff --git a/dimos/robot/manipulators/a750/blueprints.py b/dimos/robot/manipulators/a750/blueprints.py index 408f2a4a83..ccc50ac43c 100644 --- a/dimos/robot/manipulators/a750/blueprints.py +++ b/dimos/robot/manipulators/a750/blueprints.py @@ -80,7 +80,6 @@ def _a750_model_config() -> RobotModelConfig: package_paths=_A750_PACKAGE_PATHS, auto_convert_meshes=True, collision_exclusion_pairs=A750_GRIPPER_COLLISION_EXCLUSIONS, - joint_name_mapping={f"arm/joint{i}": f"joint{i}" for i in range(1, 7)}, coordinator_task_name="traj_arm", gripper_hardware_id="arm", home_joints=_A750_HOME_JOINTS, diff --git a/dimos/robot/manipulators/piper/blueprints.py b/dimos/robot/manipulators/piper/blueprints.py index 2ae06e98d5..b9f8cac4dc 100644 --- a/dimos/robot/manipulators/piper/blueprints.py +++ b/dimos/robot/manipulators/piper/blueprints.py @@ -64,7 +64,6 @@ def _piper_model_config() -> RobotModelConfig: package_paths=_PIPER_PACKAGE_PATHS, auto_convert_meshes=True, collision_exclusion_pairs=PIPER_GRIPPER_COLLISION_EXCLUSIONS, - joint_name_mapping={f"arm/joint{i}": f"joint{i}" for i in range(1, 7)}, coordinator_task_name="traj_arm", gripper_hardware_id="arm", home_joints=[0.0] * 6, diff --git a/dimos/robot/manipulators/xarm/blueprints.py b/dimos/robot/manipulators/xarm/blueprints.py index 97b3db042b..4bed0259da 100644 --- a/dimos/robot/manipulators/xarm/blueprints.py +++ b/dimos/robot/manipulators/xarm/blueprints.py @@ -23,111 +23,54 @@ dimos run keyboard-teleop-xarm7 """ -from pathlib import Path - -from dimos.control.blueprints._hardware import XARM6_FK_MODEL, XARM7_FK_MODEL, manipulator -from dimos.control.coordinator import ControlCoordinator, TaskConfig +from dimos.control.coordinator import ControlCoordinator from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.manipulation.manipulation_module import ManipulationModule -from dimos.manipulation.planning.spec.config import RobotModelConfig -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.robot.catalog.ufactory import ( + XARM6_FK_MODEL, + XARM7_FK_MODEL, + xarm6 as _catalog_xarm6, + xarm7 as _catalog_xarm7, +) from dimos.teleop.keyboard.keyboard_teleop_module import KeyboardTeleopModule -from dimos.utils.data import LfsPath - -_XARM_MODEL_PATH = LfsPath("xarm_description") / "urdf/xarm_device.urdf.xacro" -_XARM_PACKAGE_PATHS: dict[str, Path] = {"xarm_description": LfsPath("xarm_description")} - -XARM_GRIPPER_COLLISION_EXCLUSIONS: list[tuple[str, str]] = [ - ("right_inner_knuckle", "right_outer_knuckle"), - ("left_inner_knuckle", "left_outer_knuckle"), - ("right_inner_knuckle", "right_finger"), - ("left_inner_knuckle", "left_finger"), - ("left_finger", "right_finger"), - ("left_outer_knuckle", "right_outer_knuckle"), - ("left_inner_knuckle", "right_inner_knuckle"), - ("left_outer_knuckle", "right_finger"), - ("right_outer_knuckle", "left_finger"), - ("xarm_gripper_base_link", "left_inner_knuckle"), - ("xarm_gripper_base_link", "right_inner_knuckle"), - ("xarm_gripper_base_link", "left_finger"), - ("xarm_gripper_base_link", "right_finger"), - ("link6", "xarm_gripper_base_link"), - ("link6", "left_outer_knuckle"), - ("link6", "right_outer_knuckle"), -] - -def _xarm_model_config(dof: int, *, add_gripper: bool = False) -> RobotModelConfig: - xacro_args = { - "dof": str(dof), - "limited": "true", - "attach_xyz": "0.0 0.0 0.0", - "attach_rpy": "0 0.0 0", - } - if add_gripper: - xacro_args["add_gripper"] = "true" - - return RobotModelConfig( - name="arm", - model_path=_XARM_MODEL_PATH, - base_pose=PoseStamped( - position=Vector3(x=0.0, y=0.0, z=0.0), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), - ), - joint_names=[f"joint{i}" for i in range(1, dof + 1)], - end_effector_link="link_tcp" if add_gripper else f"link{dof}", - base_link="link_base", - package_paths=_XARM_PACKAGE_PATHS, - xacro_args=xacro_args, - auto_convert_meshes=True, - collision_exclusion_pairs=(XARM_GRIPPER_COLLISION_EXCLUSIONS if add_gripper else []), - joint_name_mapping={f"arm/joint{i}": f"joint{i}" for i in range(1, dof + 1)}, - coordinator_task_name="traj_arm", - gripper_hardware_id="arm" if add_gripper else None, - home_joints=[0.0] * dof, - ) - - -_xarm6_hw = manipulator( - "arm", - 6, +_xarm6_cfg = _catalog_xarm6( + name="arm", + add_gripper=False, adapter_type="xarm" if global_config.xarm6_ip else "mock", - address=global_config.xarm6_ip, + address=global_config.xarm6_ip or None, ) -_xarm7_hw = manipulator( - "arm", - 7, +_xarm7_cfg = _catalog_xarm7( + name="arm", + add_gripper=False, adapter_type="xarm" if global_config.xarm7_ip else "mock", - address=global_config.xarm7_ip, + address=global_config.xarm7_ip or None, ) # XArm6 mock sim + keyboard teleop + Drake visualization keyboard_teleop_xarm6 = autoconnect( KeyboardTeleopModule.blueprint( model_path=XARM6_FK_MODEL, - ee_joint_id=6, - joint_names=_xarm6_hw.joints, + ee_joint_id=_xarm6_cfg.dof, + joint_names=_xarm6_cfg.global_joint_names, ), ControlCoordinator.blueprint( tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[_xarm6_hw], + hardware=[_xarm6_cfg.to_hardware_component()], tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_xarm6_hw.joints, - priority=10, - params={"model_path": XARM6_FK_MODEL, "ee_joint_id": 6}, + _xarm6_cfg.to_task_config( + task_type="cartesian_ik", + task_name="cartesian_ik_arm", + model_path=XARM6_FK_MODEL, + ee_joint_id=_xarm6_cfg.dof, ), ], ), ManipulationModule.blueprint( - robots=[_xarm_model_config(6)], + robots=[_xarm6_cfg.to_robot_model_config()], visualization={"backend": "meshcat"}, ), ) @@ -136,26 +79,27 @@ def _xarm_model_config(dof: int, *, add_gripper: bool = False) -> RobotModelConf keyboard_teleop_xarm7 = autoconnect( KeyboardTeleopModule.blueprint( model_path=XARM7_FK_MODEL, - ee_joint_id=7, - joint_names=_xarm7_hw.joints, + ee_joint_id=_xarm7_cfg.dof, + joint_names=_xarm7_cfg.global_joint_names, ), ControlCoordinator.blueprint( tick_rate=100.0, publish_joint_state=True, joint_state_frame_id="coordinator", - hardware=[_xarm7_hw], + hardware=[_xarm7_cfg.to_hardware_component()], tasks=[ - TaskConfig( - name="cartesian_ik_arm", - type="cartesian_ik", - joint_names=_xarm7_hw.joints, - priority=10, - params={"model_path": XARM7_FK_MODEL, "ee_joint_id": 7}, + _xarm7_cfg.to_task_config( + task_type="cartesian_ik", + task_name="cartesian_ik_arm", + model_path=XARM7_FK_MODEL, + ee_joint_id=_xarm7_cfg.dof, ), ], ), ManipulationModule.blueprint( - robots=[_xarm_model_config(7)], + robots=[_xarm7_cfg.to_robot_model_config()], visualization={"backend": "meshcat"}, ), ) + +__all__ = ["keyboard_teleop_xarm6", "keyboard_teleop_xarm7"] diff --git a/dimos/robot/test_all_blueprints.py b/dimos/robot/test_all_blueprints.py index cdf72e9b6b..b0929ff1b6 100644 --- a/dimos/robot/test_all_blueprints.py +++ b/dimos/robot/test_all_blueprints.py @@ -49,7 +49,8 @@ "coordinator-velocity-xarm6", "coordinator-xarm6", "coordinator-xarm7", - "dual-xarm6-planner", + "dual-xarm6-planner-coordinator", + "dual-xarm7-planner-coordinator", "teleop-hosted-go2", "teleop-hosted-xarm7", "teleop-quest-dual", diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index f5ca1c536f..0ed3bbf860 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -483,6 +483,7 @@ Place your URDF/xacro files under LFS data so they can be resolved via `LfsPath` from dimos.utils.data import LfsPath from dimos.manipulation.manipulation_module import manipulation_module from dimos.manipulation.planning.spec import RobotModelConfig +from dimos.manipulation.planning.spec.models import PlanningGroupDefinition from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion from dimos.msgs.geometry_msgs.Vector3 import Vector3 @@ -506,7 +507,6 @@ def _make_base_pose(x=0.0, y=0.0, z=0.0) -> PoseStamped: def _make_yourarm_config( name: str = "arm", y_offset: float = 0.0, - joint_prefix: str = "", coordinator_task: str | None = None, ) -> RobotModelConfig: """Create YourArm robot config for planning. @@ -514,27 +514,32 @@ def _make_yourarm_config( Args: name: Robot name in the Drake planning world. y_offset: Y-axis offset for multi-arm setups. - joint_prefix: Prefix for joint name mapping to coordinator namespace. coordinator_task: Coordinator task name for trajectory execution via RPC. """ # These must match the joint names in your URDF joint_names = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] - joint_mapping = {f"{joint_prefix}{j}": j for j in joint_names} if joint_prefix else {} return RobotModelConfig( name=name, model_path=_YOURARM_URDF_PATH, - base_pose=_make_base_pose(y=y_offset), joint_names=joint_names, - end_effector_link="link6", # Last link in your URDF's kinematic chain - base_link="base_link", # Root link of your URDF + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=tuple(joint_names), + base_link="base_link", + tip_link="link6", + source="fallback", + ) + ], + base_pose=_make_base_pose(y=y_offset), # Compatibility; prefer model placement + base_link="base_link", # Compatibility robot-scoped base package_paths={"yourarm_description": _YOURARM_PACKAGE_PATH}, xacro_args={}, # Xacro arguments if using .xacro files collision_exclusion_pairs=[], # Pairs of links that can touch (e.g., gripper fingers) auto_convert_meshes=True, # Convert DAE/STL meshes for Drake max_velocity=1.0, # Max velocity scaling factor max_acceleration=2.0, # Max acceleration scaling factor - joint_name_mapping=joint_mapping, coordinator_task_name=coordinator_task, ) ``` @@ -546,7 +551,7 @@ Add this to your `dimos/robot/yourarm/blueprints.py` alongside the coordinator b ```python skip yourarm_planner = manipulation_module( - robots=[_make_yourarm_config("arm", joint_prefix="arm_", coordinator_task="traj_arm")], + robots=[_make_yourarm_config("arm", coordinator_task="traj_arm")], planning_timeout=10.0, visualization={"backend": "meshcat"}, ) @@ -560,14 +565,22 @@ yourarm_planner = manipulation_module( | Field | Description | |-------|-------------| | `model_path` | Path to `.urdf` or `.xacro` file | -| `joint_names` | Ordered list of controlled joints (must match URDF) | -| `end_effector_link` | Link to use as the end-effector for IK | -| `base_link` | Root link of the robot model | +| `joint_names` | Ordered controllable local model joint set (must match URDF); not itself a planning group | +| `planning_groups` / `srdf_path` | Explicit planning groups or SRDF source; fallback can generate `{robot_name}/manipulator` for an unambiguous single chain | | `package_paths` | Maps `package://` URIs to filesystem paths (for xacro) | -| `joint_name_mapping` | Maps coordinator names (e.g., `"arm_joint1"`) to URDF names (e.g., `"joint1"`) | | `coordinator_task_name` | Must match the `TaskConfig.name` in your coordinator blueprint | | `collision_exclusion_pairs` | List of `(link_a, link_b)` tuples for links that may legitimately touch (e.g., gripper fingers) | +Coordinator-facing joint states and trajectories use global joint names derived +mechanically as `{robot_name}/{local_joint_name}` (for example, `arm/joint1`). +Keep hardware-native name translation inside the hardware adapter; manipulation +planning config uses local model joint names. + +`base_link`, `base_pose`, and `end_effector_link` are compatibility fields used +by current placement and robot-scoped helper paths. New planning code should use +SRDF/planning-group chain base/tip links and encode robot placement in the model. See +[Planning Groups](/docs/capabilities/manipulation/planning_groups.md). + ## Step 5: Register Blueprints The blueprint registry in `dimos/robot/all_blueprints.py` is **auto-generated** by scanning the codebase for blueprint declarations. After adding your blueprints: diff --git a/docs/capabilities/manipulation/planning_groups.md b/docs/capabilities/manipulation/planning_groups.md new file mode 100644 index 0000000000..24343b5bc1 --- /dev/null +++ b/docs/capabilities/manipulation/planning_groups.md @@ -0,0 +1,123 @@ +# Manipulation Planning Groups + +Planning groups are named, selectable kinematic chains used by manipulation +planning. They separate the hardware robot identity from the part of the robot +being planned. + +## Concepts + +| Concept | Meaning | +|---------|---------| +| Planning group | A named serial chain of controllable robot joints. | +| Planning group ID | Stable API ID in the form `{robot_name}/{group_name}`. | +| Global joint name | Boundary-level joint name in the form `{robot_name}/{local_joint_name}`. | +| Generated plan | Minimal planning artifact containing selected group IDs and one synchronized global-joint path. | +| Auxiliary group | A group selected for a pose request without receiving its own pose target. | + +Local URDF/SRDF joint names stay inside robot-scoped APIs, model parsing, and +backend internals. Flat planning states and generated plan paths require global +joint names so two robots can safely have the same local joint names. + +## Planning group sources + +DimOS discovers planning groups in this order: + +1. Explicit `srdf_path` on `RobotConfig` / `RobotModelConfig`. +2. Conservative SRDF auto-discovery near the model path, with a visible warning. +3. Fallback generation of one `{robot_name}/manipulator` group if the configured + controllable joints form exactly one unambiguous serial chain. +4. Error if no SRDF exists and fallback cannot infer a single chain. + +Supported SRDF group forms: + +```xml + + + +``` + +```xml + + + + + +``` + +Unsupported SRDF forms are skipped with warnings: link groups, nested group +references, mixed group declarations, branching/non-serial groups, and SRDF +`` metadata. A chain group's `tip_link` is the pose target frame. +An ordered joint-list group may be pose-targeted only when DimOS can validate a +unique serial target frame. + +## Fallback behavior + +When no SRDF is available, fallback uses `RobotModelConfig.joint_names` as the +candidate controllable set. This field is the robot's ordered local model joint +set, not an implicit planning group. + +Fallback succeeds only when those joints form one unambiguous serial chain. It +allows prismatic joints in the middle of the chain and strips only terminal/tip +prismatic joints, which usually represent gripper fingers. The generated group +name is always `manipulator`. + +## Planning APIs + +Planning APIs select groups explicitly. Descriptors returned by +`WorldSpec.list_planning_groups()` can be passed where a group ID is accepted; +the API normalizes them back to IDs and re-resolves current world state. + +```python skip +# Joint-space planning for one group. +manip.plan_to_joint_targets({ + "left_arm/manipulator": JointState( + name=["joint1", "joint2"], + position=[0.2, -0.1], + ) +}) + +# Pose planning for an arm while a torso/waist group participates as free DOFs. +manip.plan_to_poses( + {"robot/arm": target_pose}, + auxiliary_groups=["robot/torso"], +) + +plan = manip._last_plan +manip.preview_plan(plan) +manip.execute_plan(plan) +``` + +For robot-scoped joint-space planning, unnamed vectors are interpreted in robot +model joint order. If names are provided, they must be local model joint names: +no global names, missing joints, extra joints, or partial joint sets. + +## Generated plans and execution + +A `GeneratedPlan` stores: + +- selected planning group IDs; +- a single synchronized path of `JointState` waypoints keyed by global joint + names; +- status, timing, path length, iteration count, and message metadata. + +Preview and execution project this path lazily. Preview sends projected joint +paths to the world monitor. Execution splits the path by affected trajectory +task, orders each trajectory by the robot's configured local joint order, writes +global joint names at the coordinator boundary, and invokes each trajectory +controller. Controllers remain planning-group agnostic. + +Multi-task dispatch is not atomic in this change: if one trajectory task accepts +and a later task rejects, DimOS reports the rejection but does not roll back the +accepted task. + +## Compatibility planning config fields + +`RobotConfig.base_link`, `RobotConfig.base_pose`, +`RobotModelConfig.base_link`, `RobotModelConfig.base_pose`, and +`RobotModelConfig.end_effector_link` remain as compatibility fields for the +current Drake weld/placement behavior and robot-scoped compatibility helpers. +New planning logic should use model/SRDF structure and planning group base/tip +links instead. + +Robot placement should be encoded in URDF/xacro/MJCF. `joint_names` remains +supported and should describe the ordered controllable local model joint set. diff --git a/docs/capabilities/manipulation/readme.md b/docs/capabilities/manipulation/readme.md index d991cb3f21..603f9e446e 100644 --- a/docs/capabilities/manipulation/readme.md +++ b/docs/capabilities/manipulation/readme.md @@ -129,12 +129,56 @@ touching `WorldSpec`, IK, planner objects, or live Drake contexts directly. Rend mutable joint state/path containers at the read boundary, then updates the Viser scene after manipulation/world accessors have returned. +#### Viser Planning Target Set workflow + +The Viser manipulation panel is planning-group centric. Select one or more planning groups +to form a **Planning Target Set**; IK, feasibility checks, planning, preview, execute, +clear-plan, and plan freshness are scoped to that whole set. A single xArm uses the same +workflow as a one-group target set, while dual-arm stacks can select both manipulators and +plan them together. + +- Use the planning-group checklist to add or remove groups. **Select all manipulators** + selects every planning group named `manipulator`. +- Pose target gizmos are keyed by planning group ID. Moving any selected pose gizmo triggers + whole-set IK evaluation and updates the global target joints when IK succeeds. +- Joint sliders are grouped by planning group. Editing joints triggers whole-set joint + evaluation and refreshes visible pose gizmos from FK outputs when available. +- Auxiliary groups are selected target-set members without direct gizmos. Their joints still + participate in IK seeds, target joints, feasibility, planning, preview, and execute. +- The panel exposes one Plan, Preview, Execute, Cancel, and Clear row for the whole target set; + normal operation does not expose per-robot preview or execute controls. + +Single xArm Viser example: + +```bash +uv run dimos run xarm7-planner-coordinator \ + -o manipulationmodule.visualization.backend=viser +``` + +Enable browser-panel execution only when an operator is intentionally allowed to execute plans: + +```bash +uv run dimos run xarm7-planner-coordinator \ + -o manipulationmodule.visualization.backend=viser \ + -o manipulationmodule.visualization.allow_plan_execute=true +``` + +Dual-arm mock Viser example: + +```bash +uv run dimos run dual-xarm6-planner-coordinator +``` + External manipulation visualizers are initialized from a backend-neutral planning-scene snapshot after the planning world has added its robots. This snapshot maps world robot IDs to `RobotModelConfig` metadata so Viser can prepare current, target, and transient preview robot visuals without `WorldMonitor` depending on Viser-specific hooks. Embedded Meshcat visualization does not need extra setup because it observes the Drake world directly. +Viser renders robot placement as authored in the prepared URDF/xacro output. It does not apply +`RobotModelConfig.base_pose` as an additional implicit visual transform, which avoids +double-applying placement for multi-robot models that already encode offsets in URDF/xacro. + Panel execution is opt-in. Leave `allow_plan_execute=False` unless the operator intentionally wants the browser panel to call the existing manipulation execution path. @@ -176,7 +220,7 @@ visualization backend. | `keyboard-teleop-xarm7` | XArm7 7-DOF keyboard teleop with Drake viz | | `xarm6-planner-only` | XArm6 standalone planner (no coordinator) | | `xarm7-planner-coordinator` | XArm7 planner with coordinator integration | -| `dual-xarm6-planner` | Dual XArm6 planning | +| `dual-xarm6-planner-coordinator` | Dual XArm6 planning and execution with Viser | | `xarm-perception` | XArm7 + RealSense camera for perception | | `xarm-perception-agent` | XArm7 perception + LLM agent | | `xarm-perception-sim` | XArm7 simulation perception stack | @@ -194,6 +238,14 @@ visualization backend. [guide is here](/docs/capabilities/manipulation/adding_a_custom_arm.md) +## Planning Groups + +Manipulation planning uses explicit planning group IDs such as +`arm/manipulator` and global joint names such as `arm/joint1`. See +[Planning Groups](/docs/capabilities/manipulation/planning_groups.md) for SRDF +support, fallback generation, auxiliary groups, generated plans, and execution +projection. + ## Key Files | File | Description | diff --git a/openspec/changes/add-planning-groups/.openspec.yaml b/openspec/changes/add-planning-groups/.openspec.yaml new file mode 100644 index 0000000000..494ca38369 --- /dev/null +++ b/openspec/changes/add-planning-groups/.openspec.yaml @@ -0,0 +1,2 @@ +schema: dimos-capability +created: 2026-06-13 diff --git a/openspec/changes/add-planning-groups/design.md b/openspec/changes/add-planning-groups/design.md new file mode 100644 index 0000000000..cb1fa7e8fb --- /dev/null +++ b/openspec/changes/add-planning-groups/design.md @@ -0,0 +1,555 @@ +# Planning Groups Design + +## 1. Summary + +This change makes **Planning Group** a first-class manipulation planning concept in DimOS. + +Today, manipulation planning is centered on robot identity: planner and IK interfaces select a `WorldRobotID`, while `RobotModelConfig` carries one `joint_names`, one `base_link`, and one `end_effector_link`. That shape works for a single serial arm, but it conflates robot/model identity with the kinematic subset being planned. + +The new design separates those concerns: + +- Robot identity describes the hardware/model instance. +- Planning group identity describes the selectable kinematic planning unit. +- SRDF `` declarations are the primary source of planning groups. +- Existing single-chain robots without SRDF can continue through conservative fallback generation of `{robot_name}/manipulator`. +- Planning and IK APIs select planning groups, not robot IDs. +- Pose planning supports request-scoped auxiliary groups, such as torso/waist joints that contribute free DOFs without direct pose constraints. +- Generated plans store only selected group IDs and one synchronized resolved-joint path. +- Preview and execution project from that generated plan lazily. + +The design also standardizes public joint naming above model parsing: all public joint states and paths use resolved joint names of the form `{robot_name}/{local_joint_name}`. + +## 2. Motivation + +Current manipulation planning treats a robot instance as the planning unit. `PlannerSpec.plan_joint_path(...)`, `KinematicsSpec.solve(...)`, and several `WorldSpec` methods all center on `WorldRobotID`. `ManipulationModule` also stores planned paths and trajectories by `RobotName`. + +That makes several important cases awkward: + +- Arm-only planning versus torso+arm planning. +- Coordinated dual-arm planning. +- Future multi-robot coordinated planning. +- Robots with multiple selectable serial chains. +- Explicit distinction between controllable joints and planning groups. + +The current `RobotModelConfig` fields also hide planning-group semantics. A single `end_effector_link` and `base_link` imply one planning chain per robot config. A single `joint_names` list currently acts like a hidden planning group, while also serving as the controllable/coordinator joint set. + +Finally, local URDF joint names are ambiguous once multiple robots or arms with repeated names participate in one plan. Public state/path APIs need stable resolved names so a path can represent coordinated multi-group or multi-robot motion without relying on external robot scoping. + +## 3. Goals and Non-Goals + +### Goals + +- Make planning groups first-class planning units. +- Use SRDF `` declarations as the primary source of planning group definitions. +- Provide conservative fallback generation for current single-chain arms without SRDF. +- Require explicit planning group selection for planning and IK. +- Support coordinated planning over one or more selected planning groups. +- Support pose planning with request-scoped auxiliary groups that contribute free DOFs. +- Expose stable resolved joint names above model/SRDF parsing. +- Add group-aware IK and planner interfaces. +- Replace robot-scoped end-effector FK/Jacobian queries with group-scoped APIs. +- Store minimal generated plan artifacts and project lazily for preview/execution. +- Keep existing trajectory controllers unaware of planning groups. + +### Non-Goals + +- Full MoveIt SRDF support. +- First-class composite or nested planning groups. +- Mixed pose+joint target planning in one request. +- Atomic multi-task trajectory batch dispatch. +- Rollback-on-rejection for multi-task trajectory dispatch. +- Planning config placement transforms such as `base_pose`. +- Silent compatibility mode that treats old `joint_names` as an implicit planning group without validation. +- Making controllers, coordinator tasks, or hardware drivers planning-group-aware. + +## 4. Domain Model + +Root `CONTEXT.md` contains the canonical glossary for this change. The core design shape is: + +```text +PlanningGroupDefinition + model-level declaration from SRDF or fallback generation + ↓ bind to concrete robot/world +ResolvedPlanningGroup + runtime/world-bound group with resolved joints and link frames + ↓ selected by request +PlanningGroupSelection + one or more non-overlapping resolved groups + ↓ IK/planner solve +GeneratedPlan + selected group IDs + synchronized resolved-joint path +``` + +Important terms: + +- **Planning Group**: named selectable serial kinematic chain of robot joints used as the manipulation planning unit. +- **Planning Group Definition**: model-level declaration before binding to a concrete robot/world. +- **Resolved Planning Group**: runtime/world-bound group with concrete robot identity, namespace, resolved joints, and frame data. +- **Planning Group Selection**: one or more planning groups chosen for a planning request. +- **Auxiliary Planning Group**: group selected in a specific pose-planning request without receiving a direct pose constraint in that request. +- **Planning Group ID**: public identifier, always `{robot_name}/{group_name}`. +- **Planning Group Descriptor**: immutable query snapshot describing an available planning group; not a live handle. +- **Local Model Joint Name**: name inside URDF/SRDF, such as `joint1`. +- **Resolved Joint Name**: public world-level name, always `{robot_name}/{local_joint_name}`. + +Identifier layering: + +```text +robot_name Stable robot/model instance name. +WorldRobotID Internal runtime world handle only. +PlanningGroupID {robot_name}/{group_name} +Local joint name joint1 +Resolved joint name {robot_name}/{local_joint_name} +Coordinator name Hardware/control boundary name; default identity with resolved name. +``` + +`WorldRobotID` must not appear in public state, path, generated plan, or planning group selection APIs. + +## 5. Planning Group Sources + +Planning group definitions are discovered in this precedence order: + +1. Explicit `srdf_path` on robot/model config. +2. Conservative SRDF auto-discovery with visible warning. +3. Fallback single-chain generation. +4. Error if no supported SRDF groups exist and fallback cannot infer exactly one unambiguous serial chain. + +Supported SRDF forms for this change: + +```xml + + + +``` + +```xml + + + + + +``` + +Unsupported forms are skipped with warnings: + +- `` groups. +- Nested `` references. +- Mixed link/joint/chain forms. +- Branching, disconnected, unordered, or otherwise non-serial groups. +- SRDF `` metadata. + +If a caller later selects a skipped group, resolution fails as unknown or unsupported. + +## 6. Fallback Group Generation + +When no SRDF is available, DimOS may generate exactly one planning group: + +```text +group name: manipulator +group ID: {robot_name}/manipulator +``` + +Fallback rules: + +- Use `RobotModelConfig.joint_names` as the candidate controllable set. +- Validate that candidate joints form one unambiguous serial chain in the parsed model. +- Allow prismatic joints inside the serial chain. +- Exclude only terminal/tip prismatic joints when they are likely finger/gripper joints. +- Set fallback pose target tip to the last controlled chain link. +- Error for ambiguous, branching, disconnected, or multi-chain models; such robots require SRDF. + +This preserves current single serial arm behavior without pretending every robot's `joint_names` list is a valid planning group. + +## 7. End-Effector Semantics + +A planning group is defined by chain/joints, not by SRDF `` metadata. This change ignores SRDF `` entirely. + +Rules: + +- Chain-defined group: pose target frame is the chain `tip_link`. +- Explicit joint-list group: may have a pose target frame only if validation proves it is one serial chain with a unique tip. +- Group with no valid tip: may participate in joint planning or as an auxiliary group, but cannot be directly pose-targeted. + +Pose-targeted APIs validate only targeted groups. Auxiliary groups do not need to be pose-ineligible; auxiliary status is request-scoped. A group may have a tip and still be auxiliary in a particular request. + +## 8. Public Planning APIs + +Representative API shape for pose targets: + +```python +plan_to_poses( + pose_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, PoseStamped], + *, + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroupDescriptor] = (), +) -> GeneratedPlan +``` + +Meaning: + +- `pose_targets` are selected groups with end-effector pose constraints in this request. +- `auxiliary_groups` are selected groups with no pose constraint in this request. +- Effective selection is `pose_targets.keys() ∪ auxiliary_groups`. +- Auxiliary groups are free DOFs for IK and planning. +- Mixed pose+joint targets are not supported in this change. + +Example: + +```python +plan_to_poses( + pose_targets={"robot/arm": target_pose}, + auxiliary_groups=["robot/torso"], +) +``` + +This plans one coordinated problem over arm and torso joints. The arm tip must reach `target_pose`; torso joints are free to move as needed. + +Representative API shape for joint targets: + +```python +plan_to_joint_targets( + joint_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, JointState], +) -> GeneratedPlan +``` + +Rules: + +- Joint targets are keyed by planning group at the API boundary. +- Each group's joint target keys must exactly match that group's selected resolved joints. +- Internally the request lowers to one ordered combined resolved-joint start/goal problem. + +Planning APIs may accept either a Planning Group ID string or a Planning Group Descriptor. Descriptors are ergonomic immutable snapshots. APIs normalize descriptors by extracting their ID and re-resolving current runtime group data. + +## 9. Spec Interface Changes + +This section refers to DimOS Python `Spec` Protocols, not OpenSpec behavior specs. + +### WorldSpec + +`WorldSpec` owns planning group listing and resolution: + +```python +list_planning_groups() -> Sequence[PlanningGroupDescriptor] +resolve_planning_groups(group_ids: Sequence[PlanningGroupID]) -> Sequence[ResolvedPlanningGroup] +``` + +Resolution responsibilities: + +- Validate IDs are known. +- Bind model-level definitions to concrete robot/world data. +- Convert local model joint names to resolved joint names. +- Detect overlapping resolved joints across selected groups and fail. +- Return enough resolved data for IK/planner/backend internals to map back to local names and model instances. + +Group-scoped query APIs replace robot-scoped end-effector APIs: + +```python +get_group_pose(ctx, group_id) -> PoseStamped +get_group_jacobian(ctx, group_id) -> ... +``` + +These are valid only for groups with a pose target frame. `get_link_pose(ctx, robot_id, link_name)` remains as a lower-level query. + +### KinematicsSpec + +IK must solve over the full effective planning group selection: + +```python +solve_pose_targets( + world: WorldSpec, + pose_targets: Mapping[PlanningGroupID | PlanningGroupDescriptor, PoseStamped], + *, + auxiliary_groups: Sequence[PlanningGroupID | PlanningGroupDescriptor] = (), + seed: JointState | None = None, + tolerances: PoseTolerance | None = None, + check_collision: bool = True, + max_attempts: int = 1, +) -> IKResult +``` + +IK constraints apply only to `pose_targets`. Auxiliary group joints are decision variables with no direct pose constraints. `IKResult.solution` contains exactly the selected resolved joints, not full robot/world state. + +### PlannerSpec + +Planner APIs operate over selected groups / resolved selected joints rather than a single `robot_id`. + +For joint planning: + +- Start and goal `JointState` keys must exactly equal selected resolved joints. +- Missing keys, extra keys, or partial goals fail early. + +For pose planning: + +- IK returns a selected-joint goal. +- The planner solves one combined joint-space problem from selected-joint start to selected-joint goal. + +Backends may return or raise `UNSUPPORTED` for backend limitations, including cross-robot coordinated planning. The public interface permits multi-robot selections. + +## 10. State, Naming, and Exactness Rules + +Above the model/SRDF parsing layer, all joint states use resolved joint names: + +```text +{robot_name}/{local_joint_name} +``` + +Examples: + +```text +left/joint1 +right/joint1 +a750/joint3 +``` + +Local names remain inside URDF/SRDF parsing and backend internals. Backends may strip the robot namespace and use `(local_joint_name, model_instance)` internally. + +Exactness rules: + +- Joint-space start keys must exactly equal selected resolved joints. +- Joint-space goal keys must exactly equal selected resolved joints. +- No missing selected joints. +- No extra joints. +- No partial targets. +- `IKResult.solution.keys()` equals selected resolved joints. +- `PlanningResult.path[i].keys()` equals selected resolved joints for every waypoint. + +These rules prevent silently planning for the wrong group or ignoring caller-supplied joints. + +## 11. Generated Plan Model + +`GeneratedPlan` is the canonical planning artifact: + +```python +GeneratedPlan: + group_ids: tuple[PlanningGroupID, ...] + path: list[JointState] + status: PlanningStatus + planning_time: float | None + path_length: float | None + iterations: int | None + message: str +``` + +`GeneratedPlan.path[i]` contains exactly the selected resolved joints for every waypoint. + +The plan does not store: + +- Per-robot paths. +- Per-task trajectories. +- Preview samples. +- Live world/planner handles. +- Controller-specific execution plans. + +`ManipulationModule` may keep `_last_plan: GeneratedPlan | None` as convenience state, but the returned plan object is canonical. + +## 12. Preview and Execution Projection + +Preview and execution project lazily from `GeneratedPlan`. + +### Preview + +`preview_plan(plan)`: + +- Uses the selected resolved-joint path. +- Projects into visualization/world monitor data as needed. +- Does not require execution-specific trajectory precomputation. + +### Execution + +`execute_plan(plan)`: + +1. Group resolved joint names by robot namespace/coordinator task. +2. Project the combined path into one `JointTrajectory` per affected trajectory task. +3. Order trajectory positions according to the task's configured joint order. +4. Convert resolved joint names to coordinator joint names if a boundary mapping exists. +5. Invoke each trajectory task. + +The current coordinator/JTC architecture already treats task invocation as asynchronous dispatch. JTCs run concurrently in the coordinator tick loop. This change does not add atomic batch dispatch, rollback-on-rejection, or a new execution batch abstraction. + +Planning groups do not enter the controller layer. Controllers consume joint-name-keyed trajectories. + +## 13. Migration Plan + +Configuration changes: + +- Add `srdf_path` to `RobotConfig`. +- Add `srdf_path` to `RobotModelConfig`. +- Store parsed planning group definitions on `RobotModelConfig`. +- Keep `RobotConfig.joint_names` and `RobotModelConfig.joint_names`, but define them as the controllable/coordinator joint set, not a planning group. + +Fields removed or deprecated under the new design: + +- `RobotConfig.base_link` +- `RobotConfig.base_pose` +- `RobotModelConfig.base_link` +- `RobotModelConfig.base_pose` +- `RobotModelConfig.end_effector_link` + +Implementation rollout: + +1. Add planning group data model and SRDF/fallback extraction. +2. Add deterministic local/resolved joint-name helpers. +3. Update `WorldSpec` and Drake world to list and resolve planning groups. +4. Add group-scoped pose/Jacobian APIs. +5. Update `KinematicsSpec` and IK implementation to solve selected pose targets plus auxiliary free groups. +6. Update `PlannerSpec` and planner implementation to operate over selected resolved joints. +7. Replace `ManipulationModule` robot-keyed planned path/trajectory caches with `GeneratedPlan` and optional `_last_plan`. +8. Update preview and execution projection. +9. Update manipulation skills/wrappers and documentation. +10. Update existing robot configs; rely on fallback for current single serial arms unless SRDF is added. + +Generated registry updates are not expected unless implementation changes blueprint names or adds/removes blueprint module-level variables. If that happens, run: + +```bash +pytest dimos/robot/test_all_blueprints_generation.py +``` + +## 14. Validation and Errors + +Validation should fail clearly for: + +- Unknown planning group ID. +- Unsupported selected SRDF group form. +- Fallback cannot infer one serial chain. +- Selected planning groups overlap in resolved joints. +- Pose-targeted group has no valid tip/pose target frame. +- Joint target keys do not exactly match selected group joints. +- Start/goal joint states contain missing or extra selected joints. +- Backend does not support the requested coordinated planning problem. +- Cross-robot coordinated planning requested on a backend that cannot support it. + +Warnings should be emitted for: + +- SRDF auto-discovery. +- Unsupported SRDF groups skipped during parsing. + +Manual QA should cover: + +- Single-arm no-SRDF fallback planning. +- SRDF chain group planning. +- SRDF joint-list group planning. +- Arm+torso pose planning where torso is auxiliary/free. +- Multi-group result shape and no-overlap validation. +- Multi-task execution projection with distinct joint namespaces. + +## 15. Alternatives Considered + +- **Composite groups as first-class objects**: rejected. Selecting multiple groups per request is enough and avoids duplicate group modeling. +- **Bare group names**: rejected. Planning Group IDs are always namespaced as `{robot_name}/{group_name}`. +- **Robot-scoped planning API**: rejected. Robot identity and planning group selection are separate concerns. +- **Full SRDF support**: deferred. This change supports only serial chain and ordered joint-list groups. +- **Parsing SRDF ``**: rejected for this change. Group pose target frame comes from chain/joint validation, not end-effector metadata. +- **Implicit default planning group**: rejected. Planning group selection is required. +- **Treating old `joint_names` as a compatibility planning group**: rejected unless it validates through conservative fallback. +- **`include_groups` with implicit semantics**: rejected in favor of request-scoped `auxiliary_groups`. +- **Enforcing auxiliary groups to be joint-only/no-EEF**: rejected. Auxiliary status belongs to the request, not the group definition. +- **Mixed pose+joint constraints now**: deferred. +- **Precomputed execution plans**: rejected. Generated plans stay minimal; downstream projections happen lazily. +- **Atomic multi-task trajectory dispatch**: deferred. Existing task invocation is acceptable for now. + +## 16. Rollout / Implementation Phases + +Suggested implementation phases: + +1. **Data model and parsing** + - Add planning group definition/descriptor/resolved-group types. + - Add SRDF path fields. + - Implement supported SRDF group parsing. + - Implement fallback generation. + +2. **World resolution** + - Add `WorldSpec.list_planning_groups()` and `resolve_planning_groups(...)`. + - Update Drake world to bind group definitions to model instances and resolved names. + - Add overlap validation. + +3. **Group-scoped kinematics queries** + - Add `get_group_pose(...)` and `get_group_jacobian(...)`. + - Remove/deprecate robot-scoped end-effector queries. + +4. **IK and planner interfaces** + - Update IK to accept pose targets plus auxiliary groups. + - Update planner to operate over selected resolved joints. + - Enforce exact start/goal key rules. + +5. **Generated plan flow** + - Add `GeneratedPlan`. + - Replace robot-keyed planned path/trajectory caches. + - Store only optional `_last_plan` convenience state. + +6. **Preview and execution projection** + - Project generated plans to visualization as needed. + - Project generated plans to per-task `JointTrajectory` at execution time. + +7. **Robot config migration and docs** + - Update existing robot configs. + - Add docs for SRDF support, fallback generation, APIs, and naming rules. + - Update manipulation skills/wrappers. + +## 17. Safety / Simulation / Replay + +This is primarily a planning API and modeling change. It should not change low-level trajectory controller semantics. + +Hardware-facing assumptions: + +- Execution still sends `JointTrajectory` messages to existing coordinator trajectory tasks. +- Affected trajectory tasks control disjoint resolved/coordinator joint sets. +- Multi-task dispatch is fast and non-blocking. +- Trajectory tasks execute concurrently in the coordinator tick loop. +- No new hardware-safety behavior, rollback, or atomic all-or-nothing dispatch is introduced. + +Simulation and replay should mirror hardware behavior because group resolution and generated plan projection happen above hardware drivers. Existing single-arm simulation/replay stacks should continue through fallback if they form one unambiguous serial chain. + +## 18. Risks / Trade-offs + +- **API breakage:** Existing callers plan by robot name or robot ID. Mitigation: provide migration docs and temporary wrapper conveniences where appropriate. +- **Partial SRDF support confusion:** Users may expect full MoveIt SRDF semantics. Mitigation: warn on skipped groups and document supported forms clearly. +- **Fallback misclassification:** Terminal prismatic stripping may accidentally exclude a real controllable prismatic axis. Mitigation: fallback only applies to unambiguous single chains; SRDF is required for precise modeling. +- **Joint naming migration cost:** Resolved names require updates across planner results, state monitors, and execution projection. Mitigation: deterministic helper conversion and strict layering. +- **Backend capability mismatch:** Some planners may not support multi-robot coordinated planning. Mitigation: allow `UNSUPPORTED` while preserving interface semantics. +- **Dispatch skew for multiple trajectory tasks:** Sequential task invocation can create tiny start-time differences. Mitigation: accepted for now; future coordinator batch dispatch can address this if needed. + +## 19. Open Questions + +- Exact error/status enum names for unsupported groups, no target frame, overlapping groups, and backend-unsupported problems. +- Exact temporary compatibility wrappers, if any, for existing manipulation skill APIs. +- Whether to write a short ADR for removing planning config placement fields in favor of URDF/model placement. +- Whether future coordinator-level `execute_batch(...)` is needed for tighter multi-task synchronization. + +## Appendix: Design Q&A Summary + +- **Term:** Use Planning Group; avoid Move Group/movegroup. +- **Ownership:** Define groups at model/config level; resolve to runtime/world-bound groups. +- **Composites:** Do not create composite groups now; select multiple groups per request. +- **Multi-group meaning:** One coordinated joint-space problem and one synchronized result. +- **End effectors:** Groups are defined by chain/joints, not SRDF `` metadata. +- **Group declaration:** Support chain or ordered joint list; validate as serial chain. +- **Default selection:** Planning group selection is required; no implicit default selection. +- **IDs:** Planning Group ID is always `{robot_name}/{group_name}`. +- **Descriptors:** Query APIs return immutable snapshots, not live handles. +- **Selectors:** APIs may accept group ID strings or descriptors; no dedicated selector type. +- **Joint goals:** Joint target APIs are keyed by planning group at API boundary, then lowered to one ordered resolved-joint problem. +- **Resolution:** `WorldSpec` owns group listing/resolution; planner delegates resolution to world. +- **Joint state exactness:** Joint-space start/goal keys must exactly equal selected resolved joints. +- **Source of truth:** SRDF first, conservative fallback only when no SRDF is present. +- **Fallback name:** Generated group is `manipulator`. +- **Fallback source:** Use `RobotModelConfig.joint_names` as candidate controllable joints. +- **Prismatic joints:** Middle prismatic joints are allowed; terminal/tip prismatic finger joints may be excluded. +- **SRDF scope:** Parse `` only; ignore ``. +- **Unsupported SRDF:** Skip unsupported group forms with warnings. +- **SRDF discovery:** Explicit path, then warning auto-discovery, then fallback, then error. +- **Config fields:** Add `srdf_path`; keep `base_link`, `end_effector_link`, and `base_pose` as explicit compatibility fields while new planning code uses planning-group base/tip links and model-owned placement. +- **Robot placement:** Placement belongs in URDF/model, not separate planning config transforms. +- **FK/Jacobian:** Replace robot-scoped end-effector APIs with group-scoped APIs. +- **Cross-robot planning:** Interface allows it; backends may report unsupported. +- **Overlaps:** Selected groups must never overlap in resolved joints. +- **Result shape:** `PlanningResult.path` remains combined and synchronized. +- **Stored plan:** Store selected group IDs and path only; project lazily for preview/execution. +- **Resolved naming:** Above parsing, use `{robot_name}/{local_joint_name}` everywhere. +- **Coordinator mapping:** Coordinator names are a control-boundary concern; default identity with resolved names. +- **Auxiliary groups:** Auxiliary means selected without pose constraint in this request only. +- **Auxiliary DOFs:** Auxiliary groups are free DOFs for pose planning. +- **Mixed targets:** No mixed pose+joint target API in this change. +- **IK:** IK solves over pose-targeted groups plus auxiliary groups. +- **IK result:** `IKResult.solution` contains exactly selected resolved joints. +- **Planning result:** `PlanningResult.path` contains exactly selected resolved joints. +- **Execution:** Split path by trajectory task and send to existing JTCs; no new batch/rollback semantics. +- **Generated plan:** Returned plan object is canonical; module last-plan storage is convenience. diff --git a/openspec/changes/add-planning-groups/docs.md b/openspec/changes/add-planning-groups/docs.md new file mode 100644 index 0000000000..1a8ad2b45b --- /dev/null +++ b/openspec/changes/add-planning-groups/docs.md @@ -0,0 +1,45 @@ +## User-Facing Docs + +- Update `docs/usage/` manipulation planning documentation to introduce planning groups as the user-facing planning selection unit. +- Document Planning Group IDs as `{robot_name}/{group_name}` and resolved joint names as `{robot_name}/{local_joint_name}`. +- Document supported SRDF forms: + - `` + - `...` when the joints validate as one serial chain. +- Document unsupported SRDF forms and warning behavior for skipped groups. +- Document fallback behavior for robots without SRDF: one generated `{robot_name}/manipulator` group only when configured controllable joints form one unambiguous serial chain. +- Document public planning API usage: + - pose targets keyed by planning group + - request-scoped `auxiliary_groups` + - joint targets keyed by planning group + - generated plans returned as the canonical artifact +- Document lazy preview/execution flow: generated plan first, then `preview_plan(plan)` and `execute_plan(plan)` project as needed. + +## Contributor Docs + +- Update `docs/development/` manipulation/planning contributor documentation, if present, to explain: + - SRDF/fallback extraction responsibilities + - local versus resolved joint-name layering + - where group resolution belongs + - why controllers should remain planning-group agnostic +- If no dedicated manipulation contributor doc exists, add the contributor notes to the user-facing manipulation planning doc or create a short development note for planning backends. + +## Coding-Agent Docs + +- Update `docs/coding-agents/` or `AGENTS.md` only if implementation introduces new recurring coding-agent guidance. +- Likely guidance: + - do not add robot-scoped planning APIs for new manipulation work + - use explicit Planning Group IDs in examples/tests + - keep local URDF/SRDF names below parsing/backend internals + - use resolved joint names in public paths/states + +## Doc Validation + +- Run doc link validation if available: + - `uv run doclinks` +- For docs containing executable Python snippets, run the relevant markdown execution command if supported: + - `uv run md-babel-py run ` +- Run relevant tests that exercise documented examples or API snippets after implementation. + +## No Docs Needed + +Documentation is needed. This change alters public manipulation planning concepts, API examples, naming conventions, SRDF support expectations, and migration guidance for existing callers. diff --git a/openspec/changes/add-planning-groups/proposal.md b/openspec/changes/add-planning-groups/proposal.md new file mode 100644 index 0000000000..ce14a0ee70 --- /dev/null +++ b/openspec/changes/add-planning-groups/proposal.md @@ -0,0 +1,53 @@ +## Why + +DimOS manipulation planning is currently robot-centric: planner and kinematics interfaces select a `robot_id`, while `RobotModelConfig` carries a single `joint_names`, `base_link`, and `end_effector_link` shape. That works for a single serial arm, but it hides the actual planning unit and makes torso+arm, dual-arm, and coordinated multi-group planning awkward or ambiguous. + +Planning groups should be first-class. Robot identity should describe the hardware/model instance, while planning group selection should describe which kinematic chains participate in IK and motion planning. This change also makes joint naming unambiguous above the model parsing layer by using stable resolved joint names. + +## What Changes + +- Add first-class planning group definitions sourced primarily from SRDF `` entries. +- Add conservative fallback generation of one `manipulator` planning group for unambiguous single-chain robots without SRDF. +- **BREAKING**: Planning and IK APIs move from robot-ID selection to explicit planning group selection. +- **BREAKING**: Joint states and generated paths above model parsing use resolved joint names of the form `{robot_name}/{local_joint_name}`. +- Add pose planning over pose-targeted planning groups plus request-scoped auxiliary planning groups that contribute free DOFs. +- Add coordinated multi-group planning semantics: one selected joint set, one synchronized path, and no overlapping selected joints. +- Replace robot-scoped end-effector FK/Jacobian queries with group-scoped queries. +- Introduce a minimal generated plan artifact that stores selected group IDs and a combined resolved-joint path, projecting lazily for preview and execution. +- Remove planning configuration concepts that duplicate robot model structure, including robot-level planning `base_link`, `end_effector_link`, and `base_pose` fields in the new design. + +## Affected DimOS Surfaces + +- Modules/streams: + - Manipulation planning module plan/preview/execute flow. + - Planning `WorldSpec`, `KinematicsSpec`, and `PlannerSpec` interfaces. + - Drake planning world implementation and world monitor/preview integration. + - Robot model/config parsing and model-to-planning config conversion. +- Blueprints/CLI: + - Existing manipulation blueprints should continue for single serial arms through fallback group generation. + - Ambiguous, branching, or multi-arm robots require SRDF rather than silent compatibility behavior. +- Skills/MCP: + - Manipulation skills that call pose or joint planning must select planning groups explicitly or use wrapper defaults supplied by the skill layer. +- Hardware/simulation/replay: + - Execution still sends projected trajectories to existing trajectory controller tasks. + - Multi-task execution dispatches per-task trajectories; trajectory controllers own runtime concurrency. + - No new hardware-safety behavior or atomic multi-task batch dispatch is introduced. +- Docs/generated registries: + - User/developer docs for manipulation planning APIs, SRDF support, fallback generation, and joint naming need updates. + - No generated blueprint registry changes are expected unless robot config/blueprint names change during implementation. + +## Capabilities + +### New Capabilities + +- `manipulation-planning-groups`: Planning group discovery, selection, coordinated planning, IK target semantics, generated plans, and preview/execution projection. + +### Modified Capabilities + +- None. No existing OpenSpec capability specs are present in this repository checkout. + +## Impact + +This is a public manipulation planning API redesign. Existing code that plans by robot name or assumes bare local joint names will need migration to explicit planning group IDs and resolved joint names. Existing single-arm robots without SRDF should keep working through generated `{robot_name}/manipulator` groups if their configured controllable joints form one unambiguous serial chain. + +The implementation needs tests for SRDF parsing, fallback generation, group resolution, resolved joint naming, IK with auxiliary groups, exact joint-target validation, multi-group planning result shape, and lazy preview/execution projection. Documentation should emphasize the distinction between robot identity, planning group identity, local model joint names, resolved joint names, and coordinator joint names. diff --git a/openspec/changes/add-planning-groups/specs/manipulation-planning-groups/spec.md b/openspec/changes/add-planning-groups/specs/manipulation-planning-groups/spec.md new file mode 100644 index 0000000000..603a392b0e --- /dev/null +++ b/openspec/changes/add-planning-groups/specs/manipulation-planning-groups/spec.md @@ -0,0 +1,216 @@ +## ADDED Requirements + +### Requirement: Planning group discovery + +DimOS SHALL expose manipulation planning groups discovered from supported SRDF group declarations or from conservative fallback generation when no SRDF is available. + +#### Scenario: supported SRDF chain group is discovered +- **GIVEN** a robot model configuration references an SRDF containing a `` with one `` +- **WHEN** planning groups are listed for that robot +- **THEN** the chain group is available as a planning group +- **AND** its public Planning Group ID is `{robot_name}/{group_name}` + +#### Scenario: supported SRDF joint-list group is discovered +- **GIVEN** a robot model configuration references an SRDF containing a `` with an ordered list of `` entries +- **AND** those joints validate as one serial chain +- **WHEN** planning groups are listed for that robot +- **THEN** the joint-list group is available as a planning group + +#### Scenario: unsupported SRDF groups are skipped +- **GIVEN** an SRDF contains unsupported group forms such as link groups, nested group references, mixed forms, or non-serial groups +- **WHEN** planning groups are discovered +- **THEN** unsupported groups are skipped with warnings +- **AND** supported groups from the same SRDF remain available + +#### Scenario: fallback group is generated for unambiguous single-chain robot +- **GIVEN** a robot has no SRDF +- **AND** its configured controllable joints form exactly one unambiguous serial chain +- **WHEN** planning groups are listed for that robot +- **THEN** DimOS exposes one generated planning group named `manipulator` +- **AND** the group ID is `{robot_name}/manipulator` + +#### Scenario: ambiguous fallback fails +- **GIVEN** a robot has no SRDF +- **AND** its configured controllable joints are branching, disconnected, ambiguous, or otherwise not one serial chain +- **WHEN** planning groups are discovered +- **THEN** DimOS fails with an error requiring SRDF rather than silently creating an implicit group + +### Requirement: Planning group descriptors + +DimOS SHALL provide read-only planning group descriptors that identify available groups without acting as live runtime handles. + +#### Scenario: descriptor includes stable identity +- **GIVEN** a planning group exists for a robot +- **WHEN** a caller lists planning groups +- **THEN** the returned descriptor includes the Planning Group ID +- **AND** the Planning Group ID is stable and namespaced as `{robot_name}/{group_name}` + +#### Scenario: descriptor can be used as selector +- **GIVEN** a caller receives a planning group descriptor from a query API +- **WHEN** the caller passes that descriptor to a planning API +- **THEN** DimOS selects the group by descriptor ID +- **AND** DimOS re-resolves current runtime group data instead of trusting stale descriptor fields + +### Requirement: Resolved joint naming + +DimOS SHALL expose resolved joint names above the model parsing layer using `{robot_name}/{local_joint_name}`. + +#### Scenario: generated path uses resolved names +- **GIVEN** a robot named `left` has a local model joint named `joint1` +- **WHEN** DimOS returns a planning result path that includes that joint +- **THEN** the path uses `left/joint1` as the joint name +- **AND** the bare local name `joint1` is not exposed in the public path + +#### Scenario: duplicate local names remain unambiguous +- **GIVEN** two robots both contain a local model joint named `joint1` +- **WHEN** a coordinated plan includes both joints +- **THEN** the plan distinguishes them with resolved names such as `left/joint1` and `right/joint1` + +### Requirement: Planning group selection validation + +DimOS SHALL validate that selected planning groups are known and do not overlap in resolved joints. + +#### Scenario: non-overlapping groups are accepted +- **GIVEN** a planning request selects two known planning groups with disjoint resolved joints +- **WHEN** DimOS resolves the planning group selection +- **THEN** the selection is accepted +- **AND** the effective selected joint set is the union of both groups' resolved joints + +#### Scenario: overlapping groups are rejected +- **GIVEN** a planning request selects two planning groups that share at least one resolved joint +- **WHEN** DimOS resolves the planning group selection +- **THEN** the request fails before planning begins +- **AND** the error identifies overlapping selected joints or groups + +#### Scenario: unknown group is rejected +- **GIVEN** a planning request references a Planning Group ID that is not available +- **WHEN** DimOS resolves the planning group selection +- **THEN** the request fails with an unknown planning group error + +### Requirement: Pose planning with auxiliary groups + +DimOS SHALL support pose planning with pose-targeted groups and request-scoped auxiliary groups that contribute free degrees of freedom. + +#### Scenario: auxiliary torso helps arm pose planning +- **GIVEN** a pose planning request targets `robot/arm` +- **AND** the request includes `robot/torso` as an auxiliary group +- **WHEN** DimOS solves IK and planning for the request +- **THEN** the effective planning selection includes both arm and torso joints +- **AND** the arm pose target is enforced +- **AND** torso joints are free decision variables with no direct pose constraint in that request + +#### Scenario: auxiliary status is request-scoped +- **GIVEN** a planning group has a valid pose target frame +- **WHEN** that group is listed in `auxiliary_groups` for a pose planning request +- **THEN** DimOS treats it as unconstrained by pose for that request +- **AND** the same group may be directly pose-targeted in a different request + +#### Scenario: pose-targeted group without target frame is rejected +- **GIVEN** a planning group has no valid pose target frame +- **WHEN** a caller uses that group as a key in pose targets +- **THEN** DimOS rejects the request before planning begins + +### Requirement: Joint target planning exactness + +DimOS SHALL require joint target planning requests to provide exact selected resolved joint keys. + +#### Scenario: exact joint target is accepted +- **GIVEN** a joint target request selects a planning group with resolved joints `robot/joint1` and `robot/joint2` +- **WHEN** the request provides target values for exactly `robot/joint1` and `robot/joint2` +- **THEN** DimOS accepts the target for planning + +#### Scenario: missing joint target is rejected +- **GIVEN** a joint target request selects a planning group with resolved joints `robot/joint1` and `robot/joint2` +- **WHEN** the request provides a target for only `robot/joint1` +- **THEN** DimOS rejects the request as incomplete + +#### Scenario: extra joint target is rejected +- **GIVEN** a joint target request selects a planning group with resolved joints `robot/joint1` and `robot/joint2` +- **WHEN** the request also includes `robot/joint3` +- **THEN** DimOS rejects the request because the target contains joints outside the selected planning groups + +### Requirement: IK result shape + +DimOS SHALL return IK solutions containing exactly the resolved joints selected by the effective planning group selection. + +#### Scenario: IK result excludes unrelated joints +- **GIVEN** a pose request targets `robot/arm` and includes `robot/torso` as auxiliary +- **WHEN** IK succeeds +- **THEN** the IK solution contains arm and torso resolved joints +- **AND** it excludes unrelated gripper, base, or other-arm joints that were not selected + +#### Scenario: IK solves over auxiliary joints +- **GIVEN** a pose request has auxiliary groups +- **WHEN** IK attempts to satisfy pose targets +- **THEN** auxiliary group joints are available as free variables during the solve + +### Requirement: Generated plan artifact + +DimOS SHALL return a generated plan as the canonical artifact for successful planning. + +#### Scenario: generated plan contains selected groups and combined path +- **GIVEN** a planning request succeeds for one or more planning groups +- **WHEN** DimOS returns the generated plan +- **THEN** the plan includes the selected Planning Group IDs +- **AND** the plan includes one synchronized path of resolved-joint-keyed joint states + +#### Scenario: generated plan path contains exactly selected joints +- **GIVEN** a generated plan was produced for a planning group selection +- **WHEN** a caller inspects any waypoint in the path +- **THEN** that waypoint contains exactly the selected resolved joints +- **AND** unrelated joints are excluded + +#### Scenario: generated plan is reusable for downstream calls +- **GIVEN** a caller receives a generated plan +- **WHEN** the caller previews or executes the plan +- **THEN** DimOS uses the generated plan as the source artifact +- **AND** the caller does not need hidden robot-keyed planned path state + +### Requirement: Preview and execution projection + +DimOS SHALL project generated plans lazily for preview and execution without making controllers planning-group-aware. + +#### Scenario: preview projects selected path +- **GIVEN** a generated plan contains a resolved-joint path +- **WHEN** a caller previews the plan +- **THEN** DimOS projects the path into visualization using the selected joints +- **AND** preview does not require a precomputed execution trajectory stored in the plan + +#### Scenario: execution projects per trajectory task +- **GIVEN** a generated plan contains resolved joints for one or more coordinator trajectory tasks +- **WHEN** a caller executes the plan +- **THEN** DimOS projects the combined path into one trajectory per affected task +- **AND** each trajectory is ordered according to that task's configured joint order +- **AND** planning group concepts are not exposed to the trajectory controller + +#### Scenario: multi-task execution is dispatched without batch atomicity +- **GIVEN** a generated plan affects multiple trajectory tasks with disjoint joints +- **WHEN** the plan is executed +- **THEN** DimOS dispatches projected trajectories to the affected tasks +- **AND** runtime concurrency is handled by the coordinator and trajectory tasks +- **AND** DimOS does not require an atomic batch dispatch for this change + +### Requirement: Group-scoped kinematics queries + +DimOS SHALL provide group-scoped pose and Jacobian queries for planning groups with valid pose target frames. + +#### Scenario: group pose query succeeds for chain group +- **GIVEN** a planning group has a valid tip or pose target frame +- **WHEN** a caller requests the group's pose +- **THEN** DimOS returns the pose for that group target frame + +#### Scenario: group pose query fails without target frame +- **GIVEN** a planning group has no valid pose target frame +- **WHEN** a caller requests the group's pose or Jacobian +- **THEN** DimOS fails with a clear error rather than guessing a frame + +### Requirement: Backend unsupported reporting + +DimOS SHALL allow planning backends to reject coordinated planning problems they cannot support. + +#### Scenario: cross-robot planning unsupported by backend +- **GIVEN** a request selects planning groups across multiple robots +- **AND** the active backend cannot solve cross-robot coordinated planning +- **WHEN** planning is requested +- **THEN** DimOS reports the request as unsupported +- **AND** the failure occurs without sending commands to trajectory controllers diff --git a/openspec/changes/add-planning-groups/tasks.md b/openspec/changes/add-planning-groups/tasks.md new file mode 100644 index 0000000000..e8523fc4bd --- /dev/null +++ b/openspec/changes/add-planning-groups/tasks.md @@ -0,0 +1,92 @@ +## 1. Planning group data model and parsing + +- [x] 1.1 Add planning group model types for definitions, descriptors, resolved groups, and generated plans. +- [x] 1.2 Add `srdf_path` to robot/model configuration and carry it through model-to-planning config conversion. +- [x] 1.3 Implement SRDF parsing for supported `` declarations. +- [x] 1.4 Implement SRDF parsing for ordered `...` declarations that validate as one serial chain. +- [x] 1.5 Emit warnings and skip unsupported SRDF groups, including link groups, nested group references, mixed forms, and non-serial groups. +- [x] 1.6 Ignore SRDF `` metadata for planning group extraction. +- [x] 1.7 Implement conservative SRDF auto-discovery with visible warning after explicit `srdf_path` lookup fails or is absent. +- [x] 1.8 Implement fallback generation of `{robot_name}/manipulator` from `RobotModelConfig.joint_names` when no SRDF is available. +- [x] 1.9 Validate fallback as exactly one unambiguous serial chain, allowing middle prismatic joints and excluding only terminal/tip prismatic finger joints. +- [x] 1.10 Add unit tests for SRDF chain groups, joint-list groups, skipped unsupported groups, and fallback success/failure cases. + +## 2. Naming and group resolution + +- [x] 2.1 Add deterministic helpers for local model joint names to resolved joint names: `{robot_name}/{local_joint_name}`. +- [x] 2.2 Add inverse validation/stripping helper for backend internals that need local joint names. +- [x] 2.3 Update public joint-state/path surfaces above model parsing to use resolved joint names. +- [x] 2.4 Add `WorldSpec.list_planning_groups()` and return immutable planning group descriptor snapshots. +- [x] 2.5 Add `WorldSpec.resolve_planning_groups(...)` and bind definitions to concrete robot/world data. +- [x] 2.6 Validate unknown group IDs during resolution. +- [x] 2.7 Validate selected groups never overlap in resolved joints. +- [x] 2.8 Update Drake world internals to map resolved joint names to local joint names and model instances. +- [x] 2.9 Add focused tests for descriptors, stable IDs, resolved names, duplicate local joint disambiguation, unknown groups, and overlap rejection. + +## 3. Group-scoped world and kinematics APIs + +- [x] 3.1 Add group-scoped pose query API for planning groups with valid pose target frames. +- [x] 3.2 Add group-scoped Jacobian query API for planning groups with valid pose target frames. +- [x] 3.3 Preserve lower-level link pose querying for explicit robot/link lookups. +- [x] 3.4 Remove or deprecate robot-scoped end-effector FK/Jacobian APIs. +- [x] 3.5 Update `KinematicsSpec` to solve pose targets keyed by planning group plus request-scoped auxiliary groups. +- [x] 3.6 Ensure IK solves over the full effective selection and treats auxiliary group joints as free variables. +- [x] 3.7 Ensure `IKResult.solution` contains exactly selected resolved joints and excludes unrelated joints. +- [x] 3.8 Add tests for pose-targeted groups, auxiliary groups, no-target-frame rejection, and selected-joint-only IK results. + +## 4. Planner APIs and generated plan flow + +- [x] 4.1 Update planner APIs to accept planning group selection / resolved selected joints instead of a single `robot_id` planning target. +- [x] 4.2 Enforce exact start and goal `JointState` keys for joint-space planning: no missing, extra, or partial joints. +- [x] 4.3 Implement pose planning lowering from pose targets plus auxiliary groups to IK goal and combined joint-space planning. +- [x] 4.4 Allow backends to report `UNSUPPORTED` for coordinated planning problems they cannot solve. +- [x] 4.5 Add `GeneratedPlan` as the canonical returned planning artifact with selected group IDs and combined resolved-joint path. +- [x] 4.6 Ensure every `GeneratedPlan.path` waypoint contains exactly selected resolved joints. +- [x] 4.7 Replace robot-keyed planned path and planned trajectory caches in `ManipulationModule` with optional `_last_plan` convenience state. +- [x] 4.8 Add tests for joint target exactness, pose planning result shape, multi-group synchronized paths, backend unsupported reporting, and generated plan reuse. + +## 5. Preview and execution projection + +- [x] 5.1 Update preview APIs to accept an explicit `GeneratedPlan` and optionally fall back to `_last_plan` convenience state. +- [x] 5.2 Project generated plan paths lazily into visualization/world monitor calls. +- [x] 5.3 Update execution APIs to accept an explicit `GeneratedPlan` and optionally fall back to `_last_plan` convenience state. +- [x] 5.4 Project generated plan paths lazily into one `JointTrajectory` per affected coordinator trajectory task. +- [x] 5.5 Order projected trajectory positions according to each task's configured joint order. +- [x] 5.6 Convert resolved joint names to coordinator joint names at the execution boundary when a mapping exists. +- [x] 5.7 Keep trajectory controllers and coordinator tasks planning-group agnostic. +- [x] 5.8 Add tests for single-task execution projection, multi-task execution projection, task joint ordering, and preview projection. + +## 6. Robot config migration and API cleanup + +- [x] 6.1 Reframe planning-level `RobotConfig.base_link` and `RobotConfig.base_pose` usage as active compatibility behavior. +- [x] 6.2 Reframe planning-level `RobotModelConfig.base_link`, `RobotModelConfig.base_pose`, and `RobotModelConfig.end_effector_link` usage as active compatibility behavior. +- [x] 6.3 Keep and document `RobotConfig.joint_names` and `RobotModelConfig.joint_names` as controllable/coordinator joint sets, not planning groups. +- [x] 6.4 Update existing robot catalog/config entries to use SRDF where needed or rely on fallback for unambiguous single-chain arms. +- [x] 6.5 Update manipulation skills/wrappers to select planning groups explicitly or provide clear wrapper-level defaults. +- [x] 6.6 Run `pytest dimos/robot/test_all_blueprints_generation.py` if implementation changes blueprint names or generated registry inputs. + +## 7. Documentation + +- [x] 7.1 Update user-facing manipulation planning docs with planning group concepts, IDs, resolved joint names, and API examples. +- [x] 7.2 Document supported and unsupported SRDF forms plus skipped-group warning behavior. +- [x] 7.3 Document fallback generation rules and failure behavior for ambiguous robots. +- [x] 7.4 Document pose targets, auxiliary groups, joint targets, generated plans, preview, and execution flow. +- [x] 7.5 Update contributor docs or add a development note covering group resolution ownership, local/resolved naming, and controller boundaries. +- [x] 7.6 Update coding-agent docs or `AGENTS.md` only if implementation introduces recurring guidance beyond the existing design docs. + +## 8. Verification + +- [x] 8.1 Run `openspec validate add-planning-groups`. +- [x] 8.2 Run focused tests for robot config/model parsing changes. +- [x] 8.3 Run focused tests for planning group SRDF/fallback parsing. +- [x] 8.4 Run focused tests for `WorldSpec`/Drake world group resolution and group-scoped queries. +- [x] 8.5 Run focused tests for IK and planner selected-joint semantics. +- [x] 8.6 Run focused tests for `ManipulationModule` generated plan, preview, and execution projection. +- [x] 8.7 Run broader manipulation/planning pytest targets touched by the implementation. +- [x] 8.8 Run type checks for changed planning/manipulation modules if feasible: `uv run mypy dimos/` or a narrower supported target. Attempted narrow mypy; unavailable in this environment (`mypy` executable missing). +- [x] 8.9 Run docs validation commands for changed docs, including `uv run doclinks` if available. +- [x] 8.10 Run markdown snippet validation with `uv run md-babel-py run ` for docs containing executable Python examples, if available. +- [x] 8.11 Manually QA a single-arm no-SRDF fallback planning flow in replay or simulation. Covered by `dimos/e2e_tests/test_manipulation_planning_groups.py::test_single_arm_plans_and_executes_through_control_coordinator` using `openarm-mock-planner-coordinator` under `self_hosted_large`. +- [ ] 8.12 Manually QA an SRDF-backed chain group flow if a test fixture or robot config is available. +- [ ] 8.13 Manually QA arm-plus-auxiliary-group pose planning in simulation/replay if a suitable model is available. +- [x] 8.14 Manually QA a dual-arm planning flow by launching a dual-arm planning example, preferably OpenArm if available or another suitable dual-arm robot stack, then using the manipulation client to initiate one coordinated plan that selects both arms' planning groups and verifies the generated plan contains both arms' resolved joint names. Covered by `dimos/e2e_tests/test_manipulation_planning_groups.py::test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator` using `openarm-mock-planner-coordinator` under `self_hosted_large`. diff --git a/openspec/changes/add-viser-planning-target-set/.openspec.yaml b/openspec/changes/add-viser-planning-target-set/.openspec.yaml new file mode 100644 index 0000000000..d57ce332e0 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/.openspec.yaml @@ -0,0 +1,2 @@ +schema: dimos-capability +created: 2026-06-20 diff --git a/openspec/changes/add-viser-planning-target-set/design.md b/openspec/changes/add-viser-planning-target-set/design.md new file mode 100644 index 0000000000..a1d98a0712 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/design.md @@ -0,0 +1,139 @@ +## Context + +DimOS manipulation now treats planning groups as first-class planning units and stores successful results as `GeneratedPlan` artifacts over global joint names. Viser currently lags behind that model: the panel selects one robot, maintains one target, and delegates through robot-scoped convenience calls. That works for the single-arm case, but it does not express a coordinated dual-arm intent where `left_arm/manipulator` and `right_arm/manipulator` should be solved, checked, planned, previewed, and executed as one whole target set. + +The current Viser implementation already has useful pieces: in-process adapter calls, target transform controls, joint sliders, preview animation, and `VisualizationSpec` integration through `WorldMonitor`. This change should preserve those pieces while changing their semantic unit from selected robot to planning target set. + +Root `CONTEXT.md` defines the canonical language: Planning Group, Planning Group Selection, Auxiliary Planning Group, Planning Target Set, Generated Plan, Global Joint Name, and Robot Placement. This design follows those terms. For Viser placement, this change deliberately relies on URDF/xacro-authored placement and does not apply `RobotModelConfig.base_pose` inside Viser. + +## Goals / Non-Goals + +**Goals:** + +- Make the Viser panel group-centric instead of robot-centric. +- Let users establish a Planning Target Set by selecting one or more planning groups. +- Show target gizmos keyed by Planning Group ID for selected pose-targetable groups. +- Treat auxiliary planning groups as normal target-set members without assigned gizmos. +- Make joint panels, IK, feasibility, planning, preview, execute, and freshness whole-set scoped. +- Normalize pose-authored targets into global joint targets via realtime whole-set IK. +- Add multi-target Pink IK behavior for same-robot multiple frame targets and cross-robot grouped solves. +- Keep planning, preview, and execution based on the whole `GeneratedPlan` for the target set. +- Preserve the single-robot workflow as the one-group target-set case. + +**Non-Goals:** + +- Do not add a new CLI command. +- Do not make Viser apply `base_pose` or infer backend weld behavior. +- Do not make IK collision-aware; collision validation and path collision avoidance remain WorldSpec/planner responsibilities. +- Do not add atomic multi-controller execution semantics beyond existing generated-plan projection/dispatch. +- Do not expose normal per-group or per-robot Plan/Preview/Execute controls in the target-set workflow. +- Do not require named left/right/dual-arm presets; a checklist plus “select all manipulators” is enough for the first UI. + +## DimOS Architecture + +### Viser panel and scene + +The Viser panel should maintain a Planning Target Set state rather than a single selected robot state. The target set includes selected group IDs, target authoring state, last valid global target joints, whole-set status, diagnostics, plan freshness, and plan status. + +Current robot visuals and preview ghosts remain robot-keyed because rendering a plan projects to whole robot models. Target transform controls become planning-group-keyed because pose targets belong to planning groups. A pose-targetable selected group gets a gizmo such as `/targets/left_arm/manipulator`; an auxiliary selected group participates without a gizmo. + +The panel should display one unified Target Set joint panel, visually grouped by planning group. Per-group sections are views into one target set, not independent state machines. The action row operates on the whole set. + +### ManipulationModule and adapter boundary + +Viser should not implement IK or target-set feasibility semantics directly. The in-process adapter should expose whole-set operations backed by `ManipulationModule`, such as: + +- evaluate pose targets for a planning target set; +- evaluate joint targets for a planning target set; +- plan the current target set through joint-target planning; +- preview and execute the current whole generated plan. + +Evaluation results should use global joint names as the semantic output. Viser may display local labels inside group sections, but the returned target joint state must be unambiguous and directly usable by planning APIs. + +Pose-authored targets should normalize to joint targets before planning. The panel should call pose target set evaluation during gizmo edits, update the last valid target joints, then call joint target planning when the user plans. Joint edits should call joint target set evaluation, update pose gizmos through FK where available, and update whole-set feasibility. + +### Pink IK + +Pink IK should expose one multi-target pose solve behavior while grouping internally by robot model: + +- Same-robot multiple pose targets: solve one Pink configuration with multiple frame tasks. +- Cross-robot pose targets: solve one Pink problem per robot model and combine the results into one global selected joint target. + +Auxiliary groups participate in the selected joints and seed/target vector but do not receive direct frame tasks. They are still solved, checked, planned, previewed, and executed with the whole target set. + +IK should use the last valid target joints as the seed after target-set initialization. When a group is selected, its target initializes from current state. On IK failure, keep the last valid target joints, mark the whole target set invalid/stale, and disable planning. + +### Planning and execution + +Planning remains joint-target based after target-set evaluation. The whole target set lowers to `plan_to_joint_targets(...)` keyed by planning group. Preview and execute operate on the full generated plan without normal robot filtering in the panel. + +Plan freshness is whole-set scoped. When planning succeeds, store a current-joint snapshot for all selected groups. Execution is enabled only if current selected-group joints still match that snapshot within tolerance. + +### Blueprints and streams + +No stream contract changes are expected. Existing xArm planner/coordinator blueprints remain the manual QA target. Viser should work with `xarm7-planner-coordinator` and dual xArm mock planner/coordinator through visualization config overrides. + +### Skills/MCP and generated registries + +No skill/MCP tool contract changes are expected. No blueprint registry regeneration is expected unless implementation changes blueprint definitions. + +## Decisions + +1. **Use Planning Target Set as the UI semantic unit.** + - Rationale: Once groups are selected, independent per-group UI state causes discrepancies between IK, feasibility, planning, and execution. + - Alternative rejected: separate group cards with independent status/actions. + +2. **Keep Viser placement URDF-authored.** + - Rationale: xArm xacro already encodes attachment placement. Applying `base_pose` in Viser risks double placement and duplicates Drake weld heuristics. + - Alternative rejected: backend placement inference in Viser. + +3. **Key target gizmos by Planning Group ID.** + - Rationale: pose targets belong to planning groups, not robots. This avoids a future mismatch for robots with multiple pose-targetable groups. + - Alternative rejected: robot-keyed target controls. + +4. **Plan through joint targets after evaluation.** + - Rationale: The panel can use pose gizmos naturally while the planner receives one uniform global joint target set. + - Alternative rejected: separate UI modes for pose planning and joint planning. + +5. **Group Pink multi-target solving by robot internally.** + - Rationale: Same-robot multi-frame targets need one Pink configuration. Cross-robot targets can be solved per model and combined without building a combined Pinocchio model. + - Alternative deferred: building one combined Pink model across robots. + +6. **Whole-set status is canonical.** + - Rationale: The Plan button must not be enabled because individual group sections look valid while the combined target set is invalid. + - Alternative rejected: per-group canonical statuses. + +## Safety / Simulation / Replay + +Execution remains gated by existing Viser configuration (`allow_plan_execute`) and manipulation module state. The panel should not execute unless the whole plan is fresh and current selected-group joints match the planning start snapshot. + +Collision checking remains outside IK and in WorldSpec/planner behavior. IK may return kinematically valid targets that planning later rejects due to collision or unavailable path. The UI must surface this as whole-set planning failure, not per-group success. + +Manual QA should use mock xArm blueprints first, especially `xarm7-planner-coordinator` and the dual xArm planner/coordinator, before any hardware test. Hardware tests must keep execution explicitly opt-in. + +Replay behavior is not expected to change except that Viser target-set state should consume the same current joint states as the existing panel. + +## Risks / Trade-offs + +- **Realtime IK cost:** Whole-set IK on every gizmo drag can be expensive. Use latest-request-wins and debounce behavior in the existing evaluation worker pattern. +- **State complexity:** The panel state becomes richer. Mitigate by making Planning Target Set the only authoritative UI state and treating per-group displays as projections. +- **Pink multi-frame correctness:** Same-robot multi-frame IK needs careful frame-task construction and result filtering. Add focused tests before relying on the UI. +- **Cross-robot IK limitation:** Cross-robot grouping combines independent kinematic solves. Inter-robot collision is handled by planning, not IK, so some targets may solve IK but fail planning. +- **Docs drift:** `add-planning-groups` artifacts still contain older terms such as ResolvedPlanningGroup. This change should use current glossary language and avoid reviving stale terms. + +## Migration / Rollout + +1. Add target-set evaluation structures and module/adapter methods while preserving single-group behavior. +2. Update Pink IK multi-target behavior with focused tests. +3. Rework Viser panel state and scene target controls to use planning group IDs. +4. Keep existing single-arm Viser interactions working as one selected group. +5. Add dual xArm mock tests and manual QA. +6. Update user-facing documentation for the target-set workflow. + +Rollback can keep the current single-robot panel path if target-set UI is implemented behind a small internal adapter seam, but the final intended state should be group-centric only. + +## Open Questions + +- Exact Python location/name for the target-set evaluation result type. +- Whether target-set evaluation should be public RPC or only in-process adapter/module API initially. +- How much debouncing is needed for realtime whole-set IK with real robots and larger target sets. diff --git a/openspec/changes/add-viser-planning-target-set/docs.md b/openspec/changes/add-viser-planning-target-set/docs.md new file mode 100644 index 0000000000..79a95c95b3 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/docs.md @@ -0,0 +1,32 @@ +## User-Facing Docs + +- Update manipulation/Viser usage documentation to describe the Planning Target Set workflow: + - selecting one or more planning groups; + - using group-keyed target gizmos; + - understanding auxiliary groups as selected groups without direct gizmos; + - planning, previewing, and executing the whole target set; + - using the workflow with single xArm and dual xArm mock blueprints. +- If no dedicated Viser usage page exists, add the workflow to the closest manipulation usage or capability document. +- Include an example command for launching xArm planner/coordinator with Viser enabled and execution opt-in clearly marked. + +## Contributor Docs + +- Update manipulation planning contributor notes if they describe robot-scoped Viser behavior or single-target IK assumptions. +- Mention that Viser placement is URDF-authored for this workflow and that `base_pose` is not automatically applied by Viser. + +## Coding-Agent Docs + +- No required `AGENTS.md` update is expected. +- If `docs/coding-agents/` contains manipulation-specific guidance, add a short note that target-set UI state is whole-set scoped and should not reintroduce per-robot Plan/Preview/Execute state. + +## Doc Validation + +- Run link validation for changed docs if available: + - `doclinks` +- If docs contain executable Python snippets, run: + - `md-babel-py run ` +- Run normal formatting/lint checks for any touched docs if configured in the repository. + +## No Docs Needed + +Documentation changes are needed because this change introduces a new user-facing Viser manipulation workflow and changes the mental model from robot selection to planning target sets. diff --git a/openspec/changes/add-viser-planning-target-set/proposal.md b/openspec/changes/add-viser-planning-target-set/proposal.md new file mode 100644 index 0000000000..5379496033 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/proposal.md @@ -0,0 +1,53 @@ +## Why + +The current Viser manipulation panel is robot-centric: it selects one robot, edits one target, and plans through robot-scoped compatibility APIs. That is not sufficient for dual-arm and multi-group manipulation, where the user intent is a single coordinated target over a planning group selection such as `left_arm/manipulator` plus `right_arm/manipulator`. + +DimOS now has first-class planning groups and generated plans over global joint names. Viser should expose that model naturally: users select a set of planning groups, edit pose gizmos or joint targets as one target set, and plan/preview/execute one whole generated plan. This also creates a clear path for natural pose-based bimanual planning through multi-target Pink IK. + +## What Changes + +- Add a Viser **Planning Target Set** workflow built on planning group selection rather than selected robot. +- Show planning-group-keyed target gizmos for selected pose-targetable groups. +- Treat auxiliary planning groups as members of the same target set without direct gizmo targets. +- Normalize pose-authored targets through whole-set IK into global joint targets, then plan through joint-target planning. +- Add whole-set evaluation semantics for IK, FK/target pose updates, feasibility, plan freshness, preview, and execute. +- Extend Pink IK to support multi-target pose evaluation through one unified API: + - same-robot targets use one Pink solve with multiple frame tasks; + - cross-robot targets are grouped by robot and combined into one global selected joint target. +- Keep collision validation and collision-free path planning in WorldSpec/planner responsibilities, not in IK semantics. +- Keep Viser robot placement URDF-authored for this change; do not infer or apply `base_pose` in Viser. + +## Affected DimOS Surfaces + +- Modules/streams: + - `ManipulationModule` target evaluation helpers and Viser-facing adapter surface. + - Manipulation planning group registry and group-target evaluation paths. + - Pink IK pose-target solving behavior. + - Viser visualization panel, scene target controls, and runtime state. +- Blueprints/CLI: + - No new CLI command is expected. + - Existing xArm planner/coordinator blueprints should be usable with Viser for single-arm and dual-arm mock validation. +- Skills/MCP: + - No direct MCP skill behavior change is expected. +- Hardware/simulation/replay: + - Dual-arm mock xArm planning should support group-target-set preview and planning in Viser. + - Hardware execution remains gated by existing execution controls and coordinator behavior. +- Docs/generated registries: + - Update manipulation/Viser usage docs if present. + - Update OpenSpec/docs language to reflect Planning Target Set and multi-target Pink IK. + +## Capabilities + +### New Capabilities + +- `viser-planning-target-set`: Viser UI behavior for selecting planning groups, authoring whole-set targets, evaluating target sets, and planning/previewing/executing generated plans. + +### Modified Capabilities + +- `manipulation-planning-groups`: Planning group workflows gain whole-set target authoring/evaluation semantics and multi-target Pink IK support. + +## Impact + +Users get a natural dual-arm Viser workflow: select both arm planning groups, manipulate group-keyed target gizmos, watch the whole target set solve to joint targets, then plan/preview/execute one generated plan. Developers get a cleaner module/UI boundary where Viser owns editing state and `ManipulationModule` owns target-set semantics. + +Compatibility risks are mostly UI/API surface changes around the in-process Viser adapter and target evaluation helpers. Existing single-robot Viser workflows should remain usable as the one-group case of the target-set workflow. Testing should cover single-arm regression, dual xArm mock target-set selection, group-keyed gizmo visibility, whole-set IK failure/freshness behavior, Pink multi-target IK, and plan/preview/execute whole-set scope. diff --git a/openspec/changes/add-viser-planning-target-set/specs/manipulation-planning-groups/spec.md b/openspec/changes/add-viser-planning-target-set/specs/manipulation-planning-groups/spec.md new file mode 100644 index 0000000000..e50237ad44 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/specs/manipulation-planning-groups/spec.md @@ -0,0 +1,71 @@ +## ADDED Requirements + +### Requirement: Whole-set pose target evaluation + +DimOS SHALL evaluate pose targets for a planning target set and return a global selected joint target when evaluation succeeds. + +#### Scenario: dual-arm pose targets evaluate to global joints +- **GIVEN** a planning target set includes `left_arm/manipulator` and `right_arm/manipulator` +- **AND** each selected group has an assigned pose target +- **WHEN** DimOS evaluates the pose target set +- **THEN** DimOS returns a target joint state using global joint names for both selected groups +- **AND** the result can be used as the goal for joint-target planning + +#### Scenario: auxiliary group participates without pose target +- **GIVEN** a planning target set includes one pose-targeted group and one auxiliary group +- **WHEN** DimOS evaluates the pose target set +- **THEN** the auxiliary group's selected joints participate in the returned target joint state +- **AND** DimOS does not require a direct pose target for the auxiliary group + +### Requirement: Multi-target Pink IK behavior + +DimOS SHALL support multi-target pose evaluation with Pink IK for same-robot and cross-robot planning target sets. + +#### Scenario: same robot multiple frame targets +- **GIVEN** one robot has multiple selected pose-targetable planning groups +- **WHEN** DimOS evaluates pose targets for those groups using Pink IK +- **THEN** DimOS solves the targets as one same-robot multi-frame IK problem +- **AND** returns one global selected joint target for the effective planning group selection + +#### Scenario: cross robot pose targets +- **GIVEN** a planning target set contains pose targets for planning groups on different robots +- **WHEN** DimOS evaluates the target set using Pink IK +- **THEN** DimOS evaluates the targets per robot model as needed +- **AND** combines successful results into one global selected joint target for the whole target set + +#### Scenario: IK does not own collision semantics +- **GIVEN** Pink IK returns a kinematically valid target joint state +- **WHEN** collision validation or path planning is required +- **THEN** DimOS performs collision checks through the planning world or planner responsibilities +- **AND** the IK result alone is not treated as proof that execution is collision-free + +### Requirement: Whole-set joint target evaluation + +DimOS SHALL evaluate joint targets for a planning target set as one whole selected target. + +#### Scenario: joint target evaluation returns poses for selected groups +- **GIVEN** a planning target set has global target joints for selected planning groups +- **WHEN** DimOS evaluates the joint target set +- **THEN** DimOS returns whole-set validity +- **AND** returns target poses for pose-targetable selected groups when available + +#### Scenario: invalid joint target blocks whole set +- **GIVEN** any selected planning group has missing, extra, or invalid target joints +- **WHEN** DimOS evaluates the joint target set +- **THEN** the whole target set is invalid +- **AND** planning is not enabled for the target set + +### Requirement: Target-set seed continuity + +DimOS SHALL use last valid target joints as the preferred seed for subsequent whole-set IK evaluation. + +#### Scenario: current state initializes target set +- **GIVEN** a planning group is newly added to a target set +- **WHEN** DimOS initializes target-set evaluation state +- **THEN** DimOS uses current joints for that group as the initial target and seed + +#### Scenario: subsequent IK uses last valid target +- **GIVEN** a planning target set has a last valid solved target joint state +- **WHEN** a subsequent pose edit triggers IK evaluation +- **THEN** DimOS uses the last valid target joints as the preferred seed +- **AND** planning still uses actual current state as the start when a plan is requested diff --git a/openspec/changes/add-viser-planning-target-set/specs/viser-planning-target-set/spec.md b/openspec/changes/add-viser-planning-target-set/specs/viser-planning-target-set/spec.md new file mode 100644 index 0000000000..d027115563 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/specs/viser-planning-target-set/spec.md @@ -0,0 +1,115 @@ +## ADDED Requirements + +### Requirement: Planning target set selection + +Viser SHALL let users establish a planning target set by selecting one or more manipulation planning groups. + +#### Scenario: select multiple manipulator groups +- **GIVEN** a Viser manipulation scene contains `left_arm/manipulator` and `right_arm/manipulator` +- **WHEN** the user selects both planning groups +- **THEN** Viser treats them as one planning target set +- **AND** subsequent IK, feasibility, planning, preview, execute, and stale-state checks are scoped to the whole target set + +#### Scenario: select all manipulators convenience action +- **GIVEN** a Viser manipulation scene contains multiple manipulator planning groups +- **WHEN** the user chooses the select-all-manipulators action +- **THEN** all manipulator planning groups become members of the current planning target set +- **AND** the user can still adjust the selection explicitly + +### Requirement: Group-keyed target controls + +Viser SHALL key pose target controls by Planning Group ID for selected pose-targetable planning groups. + +#### Scenario: selected pose-targetable group shows gizmo +- **GIVEN** a selected planning group has a pose target frame +- **WHEN** Viser renders the planning target set +- **THEN** Viser shows a target gizmo associated with that Planning Group ID +- **AND** moving the gizmo edits that group's pose target within the whole target set + +#### Scenario: unselected group hides gizmo +- **GIVEN** a planning group is not part of the planning target set +- **WHEN** Viser renders target controls +- **THEN** Viser does not show a target gizmo for that group +- **AND** that group does not contribute target joints to the planning target set + +#### Scenario: auxiliary group has no assigned gizmo +- **GIVEN** a planning group is selected as an auxiliary member of the planning target set +- **WHEN** Viser renders target controls +- **THEN** Viser does not assign a direct pose gizmo to that auxiliary group +- **AND** the auxiliary group still participates in whole-set IK, feasibility, planning, preview, and execute behavior + +### Requirement: Target initialization from current state + +Viser SHALL initialize newly selected planning groups from current robot state. + +#### Scenario: selecting a group is motion-neutral +- **GIVEN** a planning group is not selected +- **WHEN** the user adds the group to the planning target set +- **THEN** Viser initializes that group's target joints from current joints +- **AND** if the group is pose-targetable, its target gizmo starts at the current group pose +- **AND** adding the group alone does not create a motion target away from current state + +### Requirement: Whole-set target authoring + +Viser SHALL treat pose gizmos and joint controls as views over one planning target set. + +#### Scenario: pose edit updates target joints +- **GIVEN** a planning target set has one or more selected pose-targetable groups +- **WHEN** the user moves any selected group's target gizmo +- **THEN** Viser requests whole-set IK evaluation for all active pose targets +- **AND** Viser updates the target-set joint controls from the returned global target joints when evaluation succeeds + +#### Scenario: joint edit updates pose targets +- **GIVEN** a planning target set has target joints and pose-targetable groups +- **WHEN** the user edits target joints +- **THEN** Viser requests whole-set joint target evaluation +- **AND** Viser updates visible pose gizmos from the evaluated group poses when available + +### Requirement: Whole-set status and failures + +Viser SHALL expose one canonical whole-set status for target validity and plan readiness. + +#### Scenario: IK failure keeps last valid target +- **GIVEN** a planning target set has a last valid solved target joint state +- **WHEN** realtime IK evaluation fails for a subsequent gizmo edit +- **THEN** Viser keeps the last valid target joints +- **AND** marks the planning target set invalid or stale +- **AND** disables planning until a whole-set evaluation succeeds again + +#### Scenario: per-group diagnostics are explanatory +- **GIVEN** a planning target set evaluation returns diagnostics for individual groups +- **WHEN** Viser displays target-set status +- **THEN** the whole-set status controls whether planning is enabled +- **AND** per-group diagnostics are displayed only as explanatory details + +### Requirement: Whole-set planning actions + +Viser SHALL plan, preview, execute, clear, and report freshness for the whole planning target set. + +#### Scenario: plan target set through joint targets +- **GIVEN** a planning target set has valid global target joints +- **WHEN** the user requests planning +- **THEN** Viser requests joint-target planning for the selected planning groups +- **AND** the request represents the whole target set rather than an individual robot or group + +#### Scenario: preview and execute full generated plan +- **GIVEN** planning succeeds for a planning target set +- **WHEN** the user previews or executes from Viser +- **THEN** Viser acts on the full generated plan for the target set +- **AND** the normal UI does not expose per-robot or per-group preview/execute actions + +#### Scenario: execute requires whole-set freshness +- **GIVEN** a generated plan was created for a planning target set +- **WHEN** current joints for any selected planning group drift from the plan start snapshot beyond tolerance +- **THEN** Viser marks the plan stale for the whole target set +- **AND** disables execute for the whole plan + +### Requirement: URDF-authored visual placement + +Viser SHALL rely on authored URDF/xacro placement for robot visuals in this workflow. + +#### Scenario: dual xArm visual placement is not double-applied +- **GIVEN** dual xArm robot models encode placement in their authored URDF/xacro output +- **WHEN** Viser registers the robots for visualization +- **THEN** Viser renders the prepared URDFs as authored +- **AND** Viser does not apply `base_pose` as an additional implicit visual transform diff --git a/openspec/changes/add-viser-planning-target-set/tasks.md b/openspec/changes/add-viser-planning-target-set/tasks.md new file mode 100644 index 0000000000..bfeffc9220 --- /dev/null +++ b/openspec/changes/add-viser-planning-target-set/tasks.md @@ -0,0 +1,45 @@ +## 1. Target-set evaluation model and module boundary + +- [x] 1.1 Define a target-set evaluation result shape with whole-set status, message, group IDs, global target joints, optional group diagnostics, and pose outputs for pose-targetable groups. +- [x] 1.2 Add ManipulationModule whole-set pose evaluation for `Mapping[PlanningGroupID, PoseStamped]` plus auxiliary group IDs, returning global selected joint targets. +- [x] 1.3 Add ManipulationModule whole-set joint evaluation for `Mapping[PlanningGroupID, JointState]`, including exact target validation and FK pose outputs for pose-targetable groups. +- [x] 1.4 Update the in-process Viser adapter to expose whole-set evaluation, target-set planning, whole-plan preview, and whole-plan execution helpers. +- [x] 1.5 Preserve existing one-robot behavior as the one-planning-group target-set case. + +## 2. Pink multi-target IK + +- [x] 2.1 Extend Pink IK pose-target solving to accept multiple pose-targeted planning groups in one request. +- [x] 2.2 Implement same-robot multi-frame Pink solving with multiple frame tasks in one Pink configuration. +- [x] 2.3 Implement cross-robot grouping for Pink IK by solving per robot model and combining results into one global selected joint target. +- [x] 2.4 Ensure auxiliary planning groups participate in the selected joints and seed/target result without receiving direct frame tasks. +- [x] 2.5 Keep collision semantics outside IK; surface kinematic success separately from collision/path-planning success. +- [x] 2.6 Add Pink IK tests for single-target regression, same-robot multi-frame targets, cross-robot targets, auxiliary groups, and global joint-name output. + +## 3. Viser target-set UI and scene + +- [x] 3.1 Replace robot-centric panel state with Planning Target Set state: selected group IDs, pose targets, global target joints, last valid target joints, whole-set status, diagnostics, plan status, and start snapshots. +- [x] 3.2 Add a planning-group checklist and a select-all-manipulators action. +- [x] 3.3 Make target controls planning-group-keyed; show gizmos only for selected pose-targetable groups and hide gizmos for unselected or auxiliary-only groups. +- [x] 3.4 Render one grouped Target Set joint panel with sections per selected planning group and one whole-set status/action row. +- [x] 3.5 Make pose gizmo edits trigger latest-request-wins whole-set IK evaluation, updating target joints on success and keeping last valid target joints on failure. +- [x] 3.6 Make joint edits trigger whole-set joint evaluation and update visible pose gizmos through FK outputs. +- [x] 3.7 Make Plan/Preview/Execute/Clear actions operate on the whole target set, with no normal per-robot or per-group action controls. +- [x] 3.8 Keep Viser robot placement URDF-authored; do not apply `base_pose` during scene registration. + +## 4. Documentation + +- [x] 4.1 Update manipulation/Viser user documentation with the Planning Target Set workflow and xArm launch examples. +- [x] 4.2 Document auxiliary groups as selected target-set members without assigned gizmos. +- [x] 4.3 Document that Viser uses URDF-authored placement and does not implicitly apply `base_pose`. +- [x] 4.4 Update contributor/coding-agent docs only if they currently describe robot-centric Viser planning or per-robot preview/execute state. + +## 5. Verification + +- [x] 5.1 Run `openspec validate add-viser-planning-target-set`. +- [x] 5.2 Run focused Pink IK tests, including new multi-target cases. +- [x] 5.3 Run focused Viser tests for target-set state, group-keyed gizmos, whole-set status, plan freshness, and whole-plan preview/execute actions. +- [x] 5.4 Run manipulation unit tests covering module whole-set evaluation and planning through joint targets. +- [ ] 5.5 Run xArm single-robot Viser manual QA with `xarm7-planner-coordinator` to verify the one-group workflow still works. +- [ ] 5.6 Run dual xArm mock Viser manual QA to verify selecting both manipulators, moving both gizmos, planning, previewing, and clearing the whole target set. +- [x] 5.7 Run docs validation commands for changed docs, including `doclinks` when available and `md-babel-py run ` for executable snippets. +- [x] 5.8 Run ruff/focused lint checks for touched manipulation and visualization files. diff --git a/openspec/config.yaml b/openspec/config.yaml new file mode 100644 index 0000000000..62a72bba63 --- /dev/null +++ b/openspec/config.yaml @@ -0,0 +1,45 @@ +schema: dimos-capability + +context: | + DimOS is a robotics operating system for generalist robots. Modules communicate + through typed streams (`In[T]`, `Out[T]`) over LCM, SHM, ROS, DDS, or other + transports. Blueprints compose modules into runnable robot stacks. Skills are + `@skill`-annotated RPC methods exposed to agents and MCP clients. + + Terminology boundary: + - "OpenSpec spec" means a behavior specification under `openspec/specs/`. + - "DimOS Spec" means a Python Protocol/RPC contract in `*_spec.py` files, + usually inheriting `dimos.spec.utils.Spec` and `typing.Protocol`. + Keep these separate. OpenSpec specs describe observable behavior; DimOS Specs + describe code-level module interfaces. + + OpenSpec specs should capture current behavior, user/developer-visible + outcomes, public CLI/API/tool surfaces, robot safety constraints, and testable + scenarios. Put implementation choices, class names, module wiring, generated + registry updates, and rollout details in `design.md` or `tasks.md`. + + Documentation lives in: + - `docs/usage/` for user-facing concepts and APIs. + - `docs/capabilities/` for capability and platform guides. + - `docs/development/` for contributor process. + - `docs/coding-agents/` and `AGENTS.md` for coding-agent guidance. + +rules: + proposal: + - "Identify affected DimOS surfaces: modules, streams, blueprints, CLI, skills/MCP, docs, hardware, simulation, replay, or generated registries." + - Use capability names that match behavior domains, not Python class names. + - Mark hardware safety or public API/CLI changes explicitly. + specs: + - Write behavior-first requirements; avoid implementation detail unless it is externally observable. + - Every requirement must include at least one `#### Scenario:` block with concrete observable outcomes. + - Use "OpenSpec capability spec" when prose might otherwise be confused with DimOS Python `Spec` Protocols. + design: + - Call out DimOS `Spec` Protocols, adapter Protocols, blueprint composition, stream names/types, and skill/MCP exposure when relevant. + - Mention generated files and required regeneration commands, especially `pytest dimos/robot/test_all_blueprints_generation.py` for blueprint registry changes. + - Include hardware/simulation/replay assumptions and safety constraints for robot-facing work. + docs: + - List user-facing docs, contributor docs, coding-agent docs, and AGENTS.md updates required by the change. + - Include documentation validation commands for changed docs, such as `doclinks` and `md-babel-py run ` where applicable. + tasks: + - Include verification tasks for OpenSpec validation, relevant pytest targets, type checks when needed, and manual QA through the user-facing surface. + - Add registry generation tasks when blueprint names, module classes, or generated registry inputs change. diff --git a/openspec/schemas/dimos-capability/schema.yaml b/openspec/schemas/dimos-capability/schema.yaml new file mode 100644 index 0000000000..fedb7964ee --- /dev/null +++ b/openspec/schemas/dimos-capability/schema.yaml @@ -0,0 +1,128 @@ +name: dimos-capability +version: 1 +description: DimOS capability workflow - proposal → specs/design/docs → tasks +artifacts: + - id: proposal + generates: proposal.md + description: DimOS change proposal covering intent, scope, capability impact, and affected robot/software surfaces + template: proposal.md + instruction: | + Create the proposal document that establishes WHY this change is needed and what DimOS behavior it affects. + + Sections: + - **Why**: 1-2 concise paragraphs on the problem or opportunity. Explain why the change matters now. + - **What Changes**: Bullet list of added, modified, or removed behavior. Mark public API/CLI or hardware-safety breaking changes with **BREAKING**. + - **Affected DimOS Surfaces**: Identify modules, streams, blueprints, CLI commands, skills/MCP tools, docs, hardware, simulation, replay, generated registries, or external protocols touched by the change. + - **Capabilities**: Identify which OpenSpec capability specs will be created or modified: + - **New Capabilities**: List behavior domains introduced by the change. Each becomes `specs//spec.md`. Use kebab-case names (for example, `agent-skills-mcp`, `blueprint-composition`, `manipulation-stack`). + - **Modified Capabilities**: List existing `openspec/specs//` entries whose requirements change. Only include spec-level behavior changes, not implementation-only refactors. + - **Impact**: Summarize user/developer impact, compatibility risks, dependency changes, documentation updates, and test/QA scope. + + Keep proposals concise. Do not include line-by-line implementation details; put architecture and rollout decisions in `design.md`. + requires: [] + - id: specs + generates: specs/**/*.md + description: Behavior-first OpenSpec capability delta specifications + template: spec.md + instruction: | + Create OpenSpec capability specs that define WHAT DimOS should do, not how it is implemented. + + Create one delta spec file per capability listed in proposal.md: + - New capabilities: use `specs//spec.md` with the exact kebab-case name from the proposal. + - Modified capabilities: use the existing folder from `openspec/specs//`. + + Use these delta sections as `##` headers: + - **ADDED Requirements**: New externally observable behavior. + - **MODIFIED Requirements**: Changed behavior. Include the full updated requirement block, not a partial patch. + - **REMOVED Requirements**: Deprecated behavior. Include **Reason** and **Migration**. + - **RENAMED Requirements**: Name-only changes. Use FROM:/TO: format. + + Requirement format: + - Use `### Requirement: `. + - Use SHALL/MUST for normative requirements. + - Include at least one `#### Scenario: ` per requirement. Scenario headings MUST use exactly four `#` characters. + - Prefer `- **GIVEN**`, `- **WHEN**`, `- **THEN**`, and `- **AND**` bullets. + - Cover happy path plus meaningful edge/error/safety cases. + + DimOS-specific guidance: + - Specify user/developer-visible behavior, robot outcomes, CLI behavior, skill/MCP tool behavior, stream contracts, safety constraints, and compatibility expectations. + - Avoid Python class names, private module internals, transport implementation choices, and generated-file details unless those details are observable API contracts. + - Use "OpenSpec capability spec" in prose when needed to avoid confusion with DimOS Python `Spec` Protocols. + - If the behavior only changes implementation and not observable requirements, do not create a spec delta. + requires: + - proposal + - id: design + generates: design.md + description: DimOS technical design and architecture decisions + template: design.md + instruction: | + Create the design document that explains HOW the change should be implemented in DimOS. + + Include design.md for cross-module changes, new robot/hardware integration, new public interfaces, new dependencies, safety-sensitive behavior, generated registry changes, or unclear architecture. + + Sections: + - **Context**: Current state, relevant modules/blueprints/docs, and constraints. + - **Goals / Non-Goals**: What the design achieves and explicitly excludes. + - **DimOS Architecture**: Modules, streams, transports, blueprints, RPC/module refs, DimOS `Spec` Protocols, adapter Protocols, skills/MCP exposure, CLI entry points, and generated registries involved. + - **Decisions**: Key choices with rationale and alternatives considered. + - **Safety / Simulation / Replay**: Hardware assumptions, sim/replay behavior, safety constraints, and manual QA surface. + - **Risks / Trade-offs**: Known risks and mitigations. + - **Migration / Rollout**: Compatibility, generated files, docs, and deployment steps. + - **Open Questions**: Outstanding decisions or unknowns. + + Reference proposal.md for intent and specs for behavior. Keep line-by-line work in tasks.md. + requires: + - proposal + - id: docs + generates: docs.md + description: Documentation impact plan for user, contributor, and coding-agent docs + template: docs.md + instruction: | + Create the documentation impact plan for the change. + + Sections: + - **User-Facing Docs**: Updates under `docs/usage/`, `docs/capabilities/`, `docs/platforms/`, or README files. + - **Contributor Docs**: Updates under `docs/development/`. + - **Coding-Agent Docs**: Updates under `docs/coding-agents/` or `AGENTS.md`. + - **Doc Validation**: Commands needed for changed docs, such as `doclinks`, `md-babel-py run `, and `bin/gen-diagrams`. + - **No Docs Needed**: If no docs are needed, explain why. + + Match `docs/development/writing_docs.md`: contributor-only docs belong in `docs/development`; user-facing behavior belongs in `docs/usage` or `docs/capabilities`. + requires: + - proposal + - id: tasks + generates: tasks.md + description: Implementation, validation, docs, and manual-QA checklist + template: tasks.md + instruction: | + Create the implementation checklist. The apply phase parses checkbox format, so every actionable task MUST use `- [ ]`. + + Guidelines: + - Group tasks under numbered `##` headings. + - Each task must be `- [ ] X.Y Task description`. + - Keep tasks small enough to complete in one focused session. + - Order tasks by dependency. + - Include docs and validation tasks from docs.md. + - Include generated registry tasks when blueprints or module registry inputs change. + - Include manual QA through the actual user surface: CLI, TUI, HTTP API, MCP tool, simulation/replay blueprint, hardware procedure, or library driver. + + Typical DimOS validation tasks: + - Run `openspec validate `. + - Run focused pytest targets for changed modules. + - Run `pytest dimos/robot/test_all_blueprints_generation.py` when blueprint registry output may change. + - Run docs validation commands for changed docs. + - Run lints/types when the touched area requires them. + + Reference specs for WHAT, design for HOW, and docs.md for documentation work. + requires: + - specs + - design + - docs +apply: + requires: + - tasks + tracks: tasks.md + instruction: | + Read proposal.md, specs, design.md, docs.md, and tasks.md before editing code. + Work through pending tasks, mark checkboxes complete as they finish, and keep artifacts current when implementation changes the plan. + Verify with OpenSpec validation, focused tests, docs checks, and manual QA through the relevant DimOS surface. diff --git a/openspec/schemas/dimos-capability/templates/design.md b/openspec/schemas/dimos-capability/templates/design.md new file mode 100644 index 0000000000..25031ceb8b --- /dev/null +++ b/openspec/schemas/dimos-capability/templates/design.md @@ -0,0 +1,35 @@ +## Context + + + +## Goals / Non-Goals + +**Goals:** + + +**Non-Goals:** + + +## DimOS Architecture + + + +## Decisions + + + +## Safety / Simulation / Replay + + + +## Risks / Trade-offs + + + +## Migration / Rollout + + + +## Open Questions + + diff --git a/openspec/schemas/dimos-capability/templates/docs.md b/openspec/schemas/dimos-capability/templates/docs.md new file mode 100644 index 0000000000..d274aed653 --- /dev/null +++ b/openspec/schemas/dimos-capability/templates/docs.md @@ -0,0 +1,19 @@ +## User-Facing Docs + + + +## Contributor Docs + + + +## Coding-Agent Docs + + + +## Doc Validation + + + +## No Docs Needed + + diff --git a/openspec/schemas/dimos-capability/templates/proposal.md b/openspec/schemas/dimos-capability/templates/proposal.md new file mode 100644 index 0000000000..98d409e8de --- /dev/null +++ b/openspec/schemas/dimos-capability/templates/proposal.md @@ -0,0 +1,32 @@ +## Why + + + +## What Changes + + + +## Affected DimOS Surfaces + + +- Modules/streams: +- Blueprints/CLI: +- Skills/MCP: +- Hardware/simulation/replay: +- Docs/generated registries: + +## Capabilities + +### New Capabilities + +- ``: + +### Modified Capabilities + +- ``: + +## Impact + + diff --git a/openspec/schemas/dimos-capability/templates/spec.md b/openspec/schemas/dimos-capability/templates/spec.md new file mode 100644 index 0000000000..afc0c1ff58 --- /dev/null +++ b/openspec/schemas/dimos-capability/templates/spec.md @@ -0,0 +1,16 @@ +## ADDED Requirements + +### Requirement: + + +#### Scenario: +- **GIVEN** +- **WHEN** +- **THEN** +- **AND** + + diff --git a/openspec/schemas/dimos-capability/templates/tasks.md b/openspec/schemas/dimos-capability/templates/tasks.md new file mode 100644 index 0000000000..b38fcdfabb --- /dev/null +++ b/openspec/schemas/dimos-capability/templates/tasks.md @@ -0,0 +1,15 @@ +## 1. Implementation + +- [ ] 1.1 +- [ ] 1.2 + +## 2. Documentation + +- [ ] 2.1 + +## 3. Verification + +- [ ] 3.1 Run `openspec validate ` +- [ ] 3.2 Run focused tests for changed code +- [ ] 3.3 Run docs validation commands for changed docs +- [ ] 3.4 Manually QA through the relevant DimOS surface (CLI, MCP, simulation/replay, hardware procedure, HTTP API, or library driver) diff --git a/pyproject.toml b/pyproject.toml index b5af8dc760..ebf899fcaf 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -591,6 +591,7 @@ ignore = [ "dimos/dashboard/dimos.rbl", "dimos/web/dimos_interface/themes.json", "dimos/manipulation/manipulation_module.py", + "dimos/manipulation/planning/world/drake_world.py", "dimos/manipulation/visualization/viser/test_viser_visualization.py", "dimos/navigation/nav_stack/modules/*/main.cpp", "dimos/navigation/nav_stack/common/*.hpp",