diff --git a/dimos/manipulation/planning/groups/discovery.py b/dimos/manipulation/planning/groups/discovery.py new file mode 100644 index 0000000000..49cad388ff --- /dev/null +++ b/dimos/manipulation/planning/groups/discovery.py @@ -0,0 +1,352 @@ +# 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 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 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 + logger.warning( + 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: + logger.warning(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] + logger.warning( + 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() + logger.warning( + 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(): + logger.warning(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: + logger.warning( + 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: + logger.warning(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, + ) + + +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): + logger.warning( + 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: + logger.warning(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, + ) + + +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/identifiers.py b/dimos/manipulation/planning/groups/identifiers.py new file mode 100644 index 0000000000..db9d990081 --- /dev/null +++ b/dimos/manipulation/planning/groups/identifiers.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. + +"""Planning-group and global-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 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..c5887a090e --- /dev/null +++ b/dimos/manipulation/planning/groups/registry.py @@ -0,0 +1,108 @@ +# 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.identifiers import ( + make_global_joint_names, + make_planning_group_id, +) +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupSelection +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 unique pose-targetable group ID for robot-scoped wrappers.""" + pose_groups = [ + group for group in self.groups_for_robot(robot_name) if group.has_pose_target + ] + if not pose_groups: + return None + if len(pose_groups) > 1: + raise ValueError( + f"Robot '{robot_name}' has {len(pose_groups)} pose-targetable planning groups; " + "use an explicit planning group ID" + ) + return pose_groups[0].id diff --git a/dimos/manipulation/planning/groups/test_planning_groups.py b/dimos/manipulation/planning/groups/test_planning_groups.py new file mode 100644 index 0000000000..8b399354a6 --- /dev/null +++ b/dimos/manipulation/planning/groups/test_planning_groups.py @@ -0,0 +1,658 @@ +# 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 groups.""" + +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.manipulation.planning.groups.identifiers import local_joint_name_from_global +from dimos.manipulation.planning.groups.models import PlanningGroup, PlanningGroupDefinition +from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry +from dimos.manipulation.planning.groups.utils import ( + filter_joint_state_to_selected_joints, + joint_state_to_ordered_positions, + joint_target_to_global_names, + matching_global_joint_name, + planning_group_id_from_selector, +) +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState +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 _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 _robot_config( + name: str = "robot", + planning_groups: list[PlanningGroupDefinition] | None = None, +) -> RobotModelConfig: + return RobotModelConfig( + name=name, + model_path=Path("/tmp/robot.urdf"), + base_pose=PoseStamped(), + joint_names=["joint1", "joint2", "joint3"], + planning_groups=planning_groups + if planning_groups is not None + else [ + PlanningGroupDefinition( + name=FALLBACK_PLANNING_GROUP_NAME, + joint_names=("joint1", "joint2"), + base_link="base", + tip_link="tool", + ) + ], + ) + + +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, + """ + + + + + """, + ) + + groups = parse_srdf_planning_groups( + srdf_path, + model=model, + controllable_joint_names=["joint1", "joint2"], + ) + + assert [group.name for group in groups] == ["arm"] + + +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" + assert group.source == "fallback" + + +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_fallback_rejects_all_terminal_prismatic_candidates() -> None: + with pytest.raises(PlanningGroupDiscoveryError, match="removed all candidate joints"): + generate_fallback_planning_group( + model=_serial_model("prismatic", "prismatic"), + controllable_joint_names=["joint1", "joint2"], + ) + + +def test_parse_srdf_skips_invalid_groups_and_keeps_valid_group(tmp_path: Path) -> None: + model = _serial_model("revolute", "revolute") + srdf_path = _write_srdf( + tmp_path, + """ + + + + + """, + ) + + groups = parse_srdf_planning_groups( + srdf_path, + model=model, + controllable_joint_names=["joint1", "joint2"], + ) + + assert [group.name for group in groups] == ["arm"] + + +def test_discovery_rejects_missing_explicit_srdf(tmp_path: Path) -> None: + with pytest.raises(FileNotFoundError, match="SRDF file not found"): + discover_planning_group_definitions( + robot_name="robot", + model_path=tmp_path / "robot.urdf", + model=_serial_model("revolute"), + controllable_joint_names=["joint1"], + srdf_path=tmp_path / "missing.srdf", + ) + + +def test_discovery_falls_back_when_srdf_has_no_supported_groups(tmp_path: Path) -> None: + model_path = tmp_path / "robot.urdf.xacro" + model_path.write_text("") + (tmp_path / "robot.srdf").write_text( + "" + ) + + groups = discover_planning_group_definitions( + robot_name="robot", + model_path=model_path, + model=_serial_model("revolute"), + controllable_joint_names=["joint1"], + ) + + assert [group.name for group in groups] == [FALLBACK_PLANNING_GROUP_NAME] + assert [group.source for group in groups] == ["fallback"] + + +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(tmp_path: Path) -> None: + model = _serial_model("revolute") + model_path = tmp_path / "robot.urdf" + model_path.write_text("") + _write_srdf( + tmp_path, + "", + ) + + 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"] + + +def test_primary_pose_group_id_for_robot_raises_when_ambiguous() -> None: + registry = PlanningGroupRegistry( + [ + RobotModelConfig( + name="robot", + model_path=Path("/tmp/robot.urdf"), + base_pose=PoseStamped(), + joint_names=["joint1", "joint2"], + planning_groups=[ + PlanningGroupDefinition( + name="left", + joint_names=("joint1",), + base_link="base", + tip_link="left_tool", + ), + PlanningGroupDefinition( + name="right", + joint_names=("joint2",), + base_link="base", + tip_link="right_tool", + ), + ], + ) + ] + ) + + with pytest.raises(ValueError, match="multiple|2 pose-targetable|explicit planning group"): + registry.primary_pose_group_id_for_robot("robot") + + +def test_registry_preserves_order_and_exposes_defaults() -> None: + registry = PlanningGroupRegistry([_robot_config("left"), _robot_config("right")]) + + assert [group.id for group in registry.list()] == ["left/manipulator", "right/manipulator"] + assert registry.default_group_id_for_robot("left") == "left/manipulator" + assert registry.primary_pose_group_id_for_robot("right") == "right/manipulator" + assert registry.get("left/manipulator").source == "srdf" + assert registry.groups_for_robot("missing") == () + assert registry.default_group_id_for_robot("missing") is None + + +def test_registry_rejects_duplicate_robot_and_unknown_group() -> None: + registry = PlanningGroupRegistry([_robot_config()]) + + with pytest.raises(ValueError, match="already registered"): + registry.add_robot(_robot_config()) + with pytest.raises(KeyError, match="Unknown planning group ID"): + registry.get("robot/missing") + + +def test_selection_preserves_group_order_and_rejects_overlapping_joints() -> None: + registry = PlanningGroupRegistry( + [ + _robot_config( + planning_groups=[ + PlanningGroupDefinition("arm", ("joint1", "joint2"), "base", "tool"), + PlanningGroupDefinition("gripper", ("joint3",), "tool"), + ] + ) + ] + ) + + selection = registry.select(["robot/gripper", "robot/arm"]) + + assert selection.group_ids == ("robot/gripper", "robot/arm") + assert selection.joint_names == ("robot/joint3", "robot/joint1", "robot/joint2") + assert selection.robot_names == ("robot",) + + overlapping = ( + PlanningGroup("robot/first", "robot", "first", ("robot/joint1",), ("joint1",), "base"), + PlanningGroup("robot/second", "robot", "second", ("robot/joint1",), ("joint1",), "base"), + ) + with pytest.raises(ValueError, match="overlap"): + type(selection).from_groups(overlapping) + + +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) + + +def test_joint_target_to_global_names_rejects_bad_counts_missing_and_extra() -> None: + group = _make_group() + + with pytest.raises(ValueError, match="2 positions, expected 3"): + joint_target_to_global_names(group, JointState({"position": [1.0, 2.0]})) + with pytest.raises(ValueError, match="2 names but 3 positions"): + joint_target_to_global_names( + group, JointState({"name": ["j1", "j2"], "position": [1.0, 2.0, 3.0]}) + ) + with pytest.raises(ValueError, match="missing joints"): + joint_target_to_global_names( + group, JointState({"name": ["j1", "j2"], "position": [1.0, 2.0]}) + ) + with pytest.raises(ValueError, match="extra joints"): + joint_target_to_global_names( + group, JointState({"name": ["j1", "j2", "j3", "j4"], "position": [1.0, 2.0, 3.0, 4.0]}) + ) + + +def test_filter_joint_state_to_selected_joints_uses_local_fallbacks() -> None: + joint_state = JointState({"name": ["j1", "robot/j2"], "position": [1.0, 2.0]}) + + filtered = filter_joint_state_to_selected_joints( + joint_state, + ["robot/j1", "robot/j2"], + ["j1", "j2"], + ) + + assert filtered.name == ["robot/j1", "robot/j2"] + assert filtered.position == [1.0, 2.0] + + +def test_filter_joint_state_to_selected_joints_rejects_mismatched_and_missing_names() -> None: + joint_state = JointState({"name": ["robot/j1"], "position": [1.0]}) + + with pytest.raises(ValueError, match="same length"): + filter_joint_state_to_selected_joints(joint_state, ["robot/j1", "robot/j2"], ["j1"]) + with pytest.raises(ValueError, match="missing selected joints"): + filter_joint_state_to_selected_joints(joint_state, ["robot/j1", "robot/j2"]) + + +def test_matching_global_joint_name_requires_unique_suffix_match() -> None: + assert matching_global_joint_name({"left/j1": 1.0, "right/j2": 2.0}, "j1") == "left/j1" + assert matching_global_joint_name({"left/j1": 1.0, "right/j1": 2.0}, "j1") is None + assert matching_global_joint_name({"left/j1": 1.0}, "j2") is None + + +def test_filter_joint_state_to_selected_joints_uses_local_fallbacks() -> None: + state = JointState(name=["j2", "arm/j1"], position=[2.0, 1.0]) + + filtered = filter_joint_state_to_selected_joints( + state, + ["arm/j1", "arm/j2"], + ["j1", "j2"], + ) + + assert filtered.name == ["arm/j1", "arm/j2"] + assert filtered.position == [1.0, 2.0] + + +def test_joint_target_to_global_names_accepts_unnamed_positions_in_group_order() -> None: + target = joint_target_to_global_names( + PlanningGroup( + id="left/arm", + robot_name="left", + group_name="arm", + joint_names=("left/j2", "left/j1"), + local_joint_names=("j2", "j1"), + base_link="base", + tip_link="ee", + ), + JointState(name=[], position=[2.0, 1.0]), + ) + + assert target.name == ["left/j2", "left/j1"] + assert target.position == [2.0, 1.0] + + +def test_planning_group_id_from_selector_accepts_id_or_group() -> None: + group = _make_group() + + assert planning_group_id_from_selector(group) == "left/arm" + assert planning_group_id_from_selector("left/arm") == "left/arm" + + +def test_local_joint_name_from_global_validates_robot_prefix_and_local_shape() -> None: + assert local_joint_name_from_global("robot", "robot/j1") == "j1" + with pytest.raises(ValueError, match="does not belong"): + local_joint_name_from_global("robot", "other/j1") + with pytest.raises(ValueError, match="Invalid global joint name"): + local_joint_name_from_global("robot", "robot/") + + +def test_robot_model_config_derives_legacy_end_effector_link_from_pose_group() -> None: + config = RobotModelConfig( + name="arm", + model_path=Path("robot.urdf"), + joint_names=["j1", "j2"], + joint_name_mapping={"hw_j1": "j1", "hw_j2": "j2"}, + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=("j1", "j2"), + base_link="base", + tip_link="tool", + ) + ], + ) + + assert config.end_effector_link == "tool" + assert config.get_urdf_joint_name("hw_j1") == "j1" + assert config.get_coordinator_joint_name("j2") == "hw_j2" + assert config.get_coordinator_joint_names() == ["hw_j1", "hw_j2"] + + +def test_robot_model_config_end_effector_link_requires_pose_group() -> None: + config = RobotModelConfig( + name="arm", + model_path=Path("robot.urdf"), + joint_names=["j1"], + planning_groups=[ + PlanningGroupDefinition( + name="joint_only", + joint_names=("j1",), + base_link="base", + ) + ], + ) + + with pytest.raises(ValueError, match="no pose-target planning group"): + _ = config.end_effector_link + + +def test_robot_model_config_end_effector_link_rejects_ambiguous_pose_groups() -> None: + config = RobotModelConfig( + name="arm", + model_path=Path("robot.urdf"), + joint_names=["j1", "j2"], + planning_groups=[ + PlanningGroupDefinition( + name="left", + joint_names=("j1",), + base_link="base", + tip_link="left_tool", + ), + PlanningGroupDefinition( + name="right", + joint_names=("j2",), + base_link="base", + tip_link="right_tool", + ), + ], + ) + + with pytest.raises(ValueError, match="multiple pose-target planning groups"): + _ = config.end_effector_link + + +def test_joint_state_to_ordered_positions_accepts_all_supported_name_forms() -> None: + joint_names = ["joint1", "joint2", "joint3"] + mapping = {"hw1": "joint1", "hw2": "joint2", "hw3": "joint3"} + + unnamed = joint_state_to_ordered_positions( + JointState(name=[], position=[1.0, 2.0, 3.0]), + joint_names=joint_names, + joint_name_mapping=mapping, + ) + local = joint_state_to_ordered_positions( + JointState(name=["joint3", "joint1", "joint2"], position=[30.0, 10.0, 20.0]), + joint_names=joint_names, + joint_name_mapping=mapping, + ) + coordinator = joint_state_to_ordered_positions( + JointState(name=["hw2", "hw3", "hw1"], position=[200.0, 300.0, 100.0]), + joint_names=joint_names, + joint_name_mapping=mapping, + ) + global_names = joint_state_to_ordered_positions( + JointState(name=["arm/joint2", "arm/joint1", "arm/joint3"], position=[2.0, 1.0, 3.0]), + joint_names=joint_names, + joint_name_mapping=mapping, + ) + + assert unnamed.tolist() == [1.0, 2.0, 3.0] + assert local.tolist() == [10.0, 20.0, 30.0] + assert coordinator.tolist() == [100.0, 200.0, 300.0] + assert global_names.tolist() == [1.0, 2.0, 3.0] + + +def test_joint_state_to_ordered_positions_rejects_invalid_inputs() -> None: + joint_names = ["joint1", "joint2"] + mapping = {"hw1": "joint1"} + + with pytest.raises(ValueError, match="position length"): + joint_state_to_ordered_positions( + JointState(name=[], position=[1.0]), + joint_names=joint_names, + joint_name_mapping=mapping, + ) + with pytest.raises(ValueError, match="name and position"): + joint_state_to_ordered_positions( + JointState(name=["joint1", "joint2"], position=[1.0]), + joint_names=joint_names, + joint_name_mapping=mapping, + ) + with pytest.raises(ValueError, match="duplicate"): + joint_state_to_ordered_positions( + JointState(name=["joint1", "hw1"], position=[1.0, 2.0]), + joint_names=joint_names, + joint_name_mapping=mapping, + ) + with pytest.raises(ValueError, match="Unknown global"): + joint_state_to_ordered_positions( + JointState(name=["arm/joint3", "joint2"], position=[1.0, 2.0]), + joint_names=joint_names, + joint_name_mapping=mapping, + ) + with pytest.raises(ValueError, match="missing joints"): + joint_state_to_ordered_positions( + JointState(name=["joint1"], position=[1.0]), + joint_names=joint_names, + joint_name_mapping=mapping, + ) + with pytest.raises(ValueError, match="Unrecognized joint name"): + joint_state_to_ordered_positions( + JointState(name=["mystery", "joint2"], position=[1.0, 2.0]), + joint_names=joint_names, + joint_name_mapping=mapping, + ) diff --git a/dimos/manipulation/planning/groups/utils.py b/dimos/manipulation/planning/groups/utils.py new file mode 100644 index 0000000000..283681adcc --- /dev/null +++ b/dimos/manipulation/planning/groups/utils.py @@ -0,0 +1,180 @@ +# 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 + +import numpy as np +from numpy.typing import NDArray + +from dimos.manipulation.planning.groups.identifiers import ( + assert_global_joint_names, + assert_local_joint_names, + is_global_joint_name, +) +from dimos.manipulation.planning.groups.models import PlanningGroup +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) + + +def joint_state_to_ordered_positions( + joint_state: JointState, + *, + joint_names: Sequence[str], + joint_name_mapping: Mapping[str, str], +) -> NDArray[np.float64]: + """Convert a JointState to an array ordered by local robot joint names.""" + if not joint_state.name: + if len(joint_state.position) != len(joint_names): + raise ValueError("JointState position length must match configured joint count") + return np.asarray(joint_state.position, dtype=np.float64) + + if len(joint_state.name) != len(joint_state.position): + raise ValueError("JointState name and position lengths must match") + + joint_name_set = set(joint_names) + name_to_pos: dict[str, float] = {} + for name, position in zip(joint_state.name, joint_state.position, strict=True): + if name in joint_name_set: + resolved_name = name + elif name in joint_name_mapping: + resolved_name = joint_name_mapping[name] + elif is_global_joint_name(name): + resolved_name = name.split("/", maxsplit=1)[1] + if resolved_name not in joint_name_set: + raise ValueError(f"Unknown global joint name: {name}") + else: + raise ValueError( + f"Unrecognized joint name '{name}': not a known local name, not in joint_name_mapping, and not a global name" + ) + + if resolved_name in name_to_pos: + raise ValueError(f"JointState resolves duplicate joint '{resolved_name}'") + name_to_pos[resolved_name] = float(position) + + missing = [name for name in joint_names if name not in name_to_pos] + if missing: + raise ValueError(f"JointState missing joints: {missing}") + return np.asarray([name_to_pos[name] for name in joint_names], dtype=np.float64) diff --git a/dimos/manipulation/planning/kinematics/test_pink_ik.py b/dimos/manipulation/planning/kinematics/test_pink_ik.py index 9bfb1abeff..920f408e22 100644 --- a/dimos/manipulation/planning/kinematics/test_pink_ik.py +++ b/dimos/manipulation/planning/kinematics/test_pink_ik.py @@ -26,6 +26,7 @@ from pytest_mock import MockerFixture from dimos.manipulation.planning.factory import create_kinematics +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.kinematics.config import PinkKinematicsConfig import dimos.manipulation.planning.kinematics.pink_ik as pink_ik from dimos.manipulation.planning.kinematics.pink_ik import ( @@ -169,8 +170,15 @@ def _robot_config() -> RobotModelConfig: model_path=Path("/tmp/fake.urdf"), base_pose=PoseStamped(position=Vector3(), orientation=Quaternion(0.0, 0.0, 0.0, 1.0)), joint_names=["joint_a", "joint_b", "joint_c"], - end_effector_link="tool", base_link="base", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=("joint_a", "joint_b", "joint_c"), + base_link="base", + tip_link="tool", + ) + ], ) diff --git a/dimos/manipulation/planning/monitor/test_world_monitor.py b/dimos/manipulation/planning/monitor/test_world_monitor.py index df75d23d03..a78957858c 100644 --- a/dimos/manipulation/planning/monitor/test_world_monitor.py +++ b/dimos/manipulation/planning/monitor/test_world_monitor.py @@ -17,22 +17,59 @@ from pathlib import Path from typing import Any +import numpy as np +import pytest + from dimos.manipulation.planning import factory as planning_factory +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition 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.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 + + +class _VectorLike(list[float]): + def tolist(self) -> list[float]: + return list(self) + + +class _FakeStateMonitor: + def __init__(self, positions: list[float], stale: bool = False) -> None: + self._positions = _VectorLike(positions) + self._stale = stale + + def get_current_positions(self) -> _VectorLike: + return self._positions + + def get_current_velocities(self) -> None: + return None + + def is_state_stale(self, max_age: float) -> bool: + _ = max_age + return self._stale + + +class _ScratchContext: + def __enter__(self) -> str: + return "scratch" + + def __exit__(self, exc_type: object, exc: object, traceback: object) -> bool: + return False class FakeWorld: def __init__(self) -> None: - self.calls: list[tuple[str, Any]] = [] + self.calls: list[tuple[Any, ...]] = [] + self.configs: dict[str, RobotModelConfig] = {} def add_robot(self, config): self.calls.append(("add_robot", config)) - return "robot-1" + robot_id = f"robot-{len(self.configs) + 1}" + self.configs[robot_id] = config + return robot_id def get_robot_ids(self): return [] @@ -69,12 +106,14 @@ def get_live_context(self): return None def scratch_context(self): - return self + self.calls.append(("scratch_context", None)) + return _ScratchContext() def sync_from_joint_state(self, robot_id, joint_state) -> None: return None def set_joint_state(self, ctx, robot_id, joint_state) -> None: + self.calls.append(("set_joint_state", ctx, robot_id, joint_state)) return None def get_joint_state(self, ctx, robot_id): @@ -95,12 +134,20 @@ def check_edge_collision_free(self, robot_id, start, end, step_size: float = 0.0 def get_ee_pose(self, ctx, robot_id): return None + def get_group_ee_pose(self, ctx, group_id): + self.calls.append(("get_group_ee_pose", ctx, group_id)) + return PoseStamped(position=Vector3(1, 2, 3), orientation=Quaternion([0, 0, 0, 1])) + def get_link_pose(self, ctx, robot_id, link_name): return [] def get_jacobian(self, ctx, robot_id): return [] + def get_group_jacobian(self, ctx, group_id): + self.calls.append(("get_group_jacobian", ctx, group_id)) + return np.ones((6, 2)) + def get_visualization_url(self): return None @@ -155,8 +202,32 @@ def _robot_config() -> RobotModelConfig: model_path=Path("/tmp/arm.urdf"), base_pose=PoseStamped(position=Vector3(), orientation=Quaternion([0, 0, 0, 1])), joint_names=["j1", "j2"], - end_effector_link="ee", base_link="base", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", joint_names=("j1", "j2"), base_link="base", tip_link="ee" + ) + ], + ) + + +def _robot_config_with_groups(groups: list[PlanningGroupDefinition]) -> RobotModelConfig: + return _robot_config().model_copy(update={"planning_groups": groups}) + + +def _three_joint_reordered_group_config() -> RobotModelConfig: + return _robot_config().model_copy( + update={ + "joint_names": ["j1", "j2", "j3"], + "planning_groups": [ + PlanningGroupDefinition( + name="manipulator", + joint_names=("j2", "j1"), + base_link="base", + tip_link="ee", + ) + ], + } ) @@ -204,3 +275,217 @@ def test_create_planning_specs_wraps_existing_world(monkeypatch) -> None: assert planning_specs.world_monitor.visualization is None assert planning_specs.kinematics is fake_kinematics assert planning_specs.planner is fake_planner + + +def test_world_monitor_exposes_planning_groups_and_duplicate_names_do_not_mutate() -> None: + fake_world = FakeWorld() + monitor = world_monitor_module.WorldMonitor(world=fake_world) # type: ignore[arg-type] + monitor.add_robot(_robot_config()) + + assert [group.id for group in monitor.planning_groups.list()] == ["arm/manipulator"] + with pytest.raises(ValueError, match="already registered"): + monitor.add_robot(_robot_config()) + assert [call[0] for call in fake_world.calls].count("add_robot") == 1 + + +def test_world_monitor_invalid_duplicate_group_config_does_not_mutate_backend() -> None: + fake_world = FakeWorld() + monitor = world_monitor_module.WorldMonitor(world=fake_world) # type: ignore[arg-type] + invalid_config = _robot_config_with_groups( + [ + PlanningGroupDefinition( + name="manipulator", joint_names=("j1",), base_link="base", tip_link="ee" + ), + PlanningGroupDefinition( + name="manipulator", joint_names=("j2",), base_link="base", tip_link="ee" + ), + ] + ) + + with pytest.raises(ValueError, match="already registered"): + monitor.add_robot(invalid_config) + + assert [call[0] for call in fake_world.calls].count("add_robot") == 0 + + +def test_world_monitor_invalid_group_joint_name_does_not_mutate_backend() -> None: + fake_world = FakeWorld() + monitor = world_monitor_module.WorldMonitor(world=fake_world) # type: ignore[arg-type] + invalid_config = _robot_config_with_groups( + [ + PlanningGroupDefinition( + name="manipulator", + joint_names=("j1", "bad/joint"), + base_link="base", + tip_link="ee", + ) + ] + ) + + with pytest.raises(ValueError, match="Invalid local joint name"): + monitor.add_robot(invalid_config) + + assert [call[0] for call in fake_world.calls].count("add_robot") == 0 + + +def test_current_group_joint_state_uses_public_names_in_group_order() -> None: + fake_world = FakeWorld() + monitor = world_monitor_module.WorldMonitor(world=fake_world) # type: ignore[arg-type] + robot_id = monitor.add_robot(_three_joint_reordered_group_config()) + monitor._state_monitors[robot_id] = _FakeStateMonitor([0.1, 0.2, 0.3]) # type: ignore[attr-defined] + + state = monitor.current_group_joint_state("arm/manipulator") + + assert state.name == ["arm/j2", "arm/j1"] + assert state.position == [0.2, 0.1] + + +def test_current_global_joint_state_skips_stale_robots_and_preserves_state_order() -> None: + fake_world = FakeWorld() + monitor = world_monitor_module.WorldMonitor(world=fake_world) # type: ignore[arg-type] + fresh_id = monitor.add_robot(_three_joint_reordered_group_config()) + stale_id = monitor.add_robot( + RobotModelConfig( + name="arm2", + model_path=Path("/tmp/arm2.urdf"), + joint_names=["a", "b"], + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", joint_names=("a", "b"), base_link="base", tip_link="ee" + ) + ], + ) + ) + monitor._state_monitors[fresh_id] = _FakeStateMonitor([0.1, 0.2, 0.3]) # type: ignore[attr-defined] + monitor._state_monitors[stale_id] = _FakeStateMonitor([1.0, 2.0], stale=True) # type: ignore[attr-defined] + monitor.add_robot( + RobotModelConfig( + name="arm3", + model_path=Path("/tmp/arm3.urdf"), + joint_names=["x"], + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", joint_names=("x",), base_link="base", tip_link="ee" + ) + ], + ) + ) + + state = monitor.current_global_joint_state(max_age=0.5) + + assert state.name == ["arm/j1", "arm/j2", "arm/j3"] + assert state.position == [0.1, 0.2, 0.3] + + +def test_current_group_joint_state_rejects_stale_or_unavailable_state() -> None: + stale_world = FakeWorld() + stale_monitor = world_monitor_module.WorldMonitor(world=stale_world) # type: ignore[arg-type] + stale_id = stale_monitor.add_robot(_three_joint_reordered_group_config()) + stale_monitor._state_monitors[stale_id] = _FakeStateMonitor([0.1, 0.2, 0.3], stale=True) # type: ignore[attr-defined] + + with pytest.raises(ValueError, match="stale"): + stale_monitor.current_group_joint_state("arm/manipulator") + + unavailable_monitor = world_monitor_module.WorldMonitor(world=FakeWorld()) # type: ignore[arg-type] + unavailable_monitor.add_robot(_three_joint_reordered_group_config()) + with pytest.raises(ValueError, match="unavailable"): + unavailable_monitor.current_group_joint_state("arm/manipulator") + + +def test_group_ee_pose_uses_current_state_when_no_joint_state_is_provided() -> None: + fake_world = FakeWorld() + monitor = world_monitor_module.WorldMonitor(world=fake_world) # type: ignore[arg-type] + robot_id = monitor.add_robot(_three_joint_reordered_group_config()) + monitor._state_monitors[robot_id] = _FakeStateMonitor([0.1, 0.2, 0.3]) # type: ignore[attr-defined] + + pose = monitor.get_group_ee_pose("arm/manipulator") + + set_calls = [call for call in fake_world.calls if call[0] == "set_joint_state"] + assert set_calls[0][3].name == ["j1", "j2", "j3"] + assert set_calls[0][3].position == [0.1, 0.2, 0.3] + assert pose.position.x == 1 + + +def test_group_ee_pose_without_joint_state_rejects_stale_or_unavailable_state() -> None: + stale_world = FakeWorld() + stale_monitor = world_monitor_module.WorldMonitor(world=stale_world) # type: ignore[arg-type] + stale_id = stale_monitor.add_robot(_three_joint_reordered_group_config()) + stale_monitor._state_monitors[stale_id] = _FakeStateMonitor([0.1, 0.2, 0.3], stale=True) # type: ignore[attr-defined] + + with pytest.raises(ValueError, match="stale"): + stale_monitor.get_group_ee_pose("arm/manipulator") + + unavailable_monitor = world_monitor_module.WorldMonitor(world=FakeWorld()) # type: ignore[arg-type] + unavailable_monitor.add_robot(_three_joint_reordered_group_config()) + with pytest.raises(ValueError, match="unavailable"): + unavailable_monitor.get_group_ee_pose("arm/manipulator") + + +def test_group_kinematics_with_full_state_does_not_require_current_state() -> None: + fake_world = FakeWorld() + monitor = world_monitor_module.WorldMonitor(world=fake_world) # type: ignore[arg-type] + monitor.add_robot(_three_joint_reordered_group_config()) + + pose = monitor.get_group_ee_pose( + "arm/manipulator", + JointState(name=["j1", "j2", "j3"], position=[0.1, 0.2, 0.3]), + ) + + set_calls = [call for call in fake_world.calls if call[0] == "set_joint_state"] + assert set_calls[0][3].name == ["j1", "j2", "j3"] + assert set_calls[0][3].position == [0.1, 0.2, 0.3] + assert pose.position.x == 1 + + +def test_group_kinematics_route_full_state_to_backend() -> None: + fake_world = FakeWorld() + monitor = world_monitor_module.WorldMonitor(world=fake_world) # type: ignore[arg-type] + monitor.add_robot(_three_joint_reordered_group_config()) + + pose = monitor.get_group_ee_pose( + "arm/manipulator", + JointState(name=["j1", "j2", "j3"], position=[0.9, 0.8, 0.3]), + ) + jacobian = monitor.get_group_jacobian( + "arm/manipulator", + JointState(name=["j1", "j2", "j3"], position=[0.4, 0.3, 0.3]), + ) + + set_calls = [call for call in fake_world.calls if call[0] == "set_joint_state"] + assert set_calls[0][3].name == ["j1", "j2", "j3"] + assert set_calls[0][3].position == [0.9, 0.8, 0.3] + assert set_calls[1][3].name == ["j1", "j2", "j3"] + assert set_calls[1][3].position == [0.4, 0.3, 0.3] + assert pose.position.x == 1 + assert jacobian.shape == (6, 2) + assert ("get_group_ee_pose", "scratch", "arm/manipulator") in fake_world.calls + assert ("get_group_jacobian", "scratch", "arm/manipulator") in fake_world.calls + + +def test_legacy_wrappers_fail_for_no_pose_and_ambiguous_pose_groups() -> None: + fake_world = FakeWorld() + monitor = world_monitor_module.WorldMonitor(world=fake_world) # type: ignore[arg-type] + no_pose_id = monitor.add_robot( + _robot_config_with_groups( + [PlanningGroupDefinition(name="base", joint_names=("j1",), base_link="base")] + ) + ) + with pytest.raises(ValueError, match="no pose-targetable"): + monitor.get_ee_pose(no_pose_id, JointState(name=["j1", "j2"], position=[0.0, 0.0])) + + fake_world2 = FakeWorld() + monitor2 = world_monitor_module.WorldMonitor(world=fake_world2) # type: ignore[arg-type] + ambiguous_id = monitor2.add_robot( + _robot_config_with_groups( + [ + PlanningGroupDefinition( + name="a", joint_names=("j1",), base_link="base", tip_link="ee1" + ), + PlanningGroupDefinition( + name="b", joint_names=("j2",), base_link="base", tip_link="ee2" + ), + ] + ) + ) + with pytest.raises(ValueError, match="pose-targetable planning groups"): + monitor2.get_jacobian(ambiguous_id, JointState(name=["j1", "j2"], position=[0.0, 0.0])) diff --git a/dimos/manipulation/planning/monitor/world_monitor.py b/dimos/manipulation/planning/monitor/world_monitor.py index 5e12568874..9b54332de8 100644 --- a/dimos/manipulation/planning/monitor/world_monitor.py +++ b/dimos/manipulation/planning/monitor/world_monitor.py @@ -21,6 +21,12 @@ from typing import TYPE_CHECKING, Any from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.manipulation.planning.groups.identifiers import ( + make_global_joint_names, + make_planning_group_id, +) +from dimos.manipulation.planning.groups.registry import PlanningGroupRegistry +from dimos.manipulation.planning.groups.utils import filter_joint_state_to_selected_joints 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 @@ -40,6 +46,8 @@ CollisionObjectMessage, JointPath, Obstacle, + PlanningGroupID, + RobotName, WorldRobotID, ) from dimos.msgs.vision_msgs.Detection3D import Detection3D @@ -61,6 +69,8 @@ def __init__( self._lock = threading.RLock() self._robot_joints: dict[WorldRobotID, list[str]] = {} self._robot_configs: dict[WorldRobotID, RobotModelConfig] = {} + self._robot_ids_by_name: dict[RobotName, WorldRobotID] = {} + self._planning_groups = PlanningGroupRegistry() self._state_monitors: dict[WorldRobotID, RobotStateMonitor] = {} self._obstacle_monitor: WorldObstacleMonitor | None = None self._viz_thread: threading.Thread | None = None @@ -72,12 +82,22 @@ def __init__( def add_robot(self, config: RobotModelConfig) -> WorldRobotID: """Add a robot. Returns robot_id.""" with self._lock: + if config.name in self._robot_ids_by_name: + raise ValueError(f"Robot name '{config.name}' is already registered") + self._validate_planning_group_config(config) robot_id = self._world.add_robot(config) self._robot_joints[robot_id] = config.joint_names self._robot_configs[robot_id] = config + self._robot_ids_by_name[config.name] = robot_id + self._planning_groups.add_robot(config) logger.info(f"Added robot '{config.name}' as '{robot_id}'") return robot_id + @property + def planning_groups(self) -> PlanningGroupRegistry: + """Registered public planning groups.""" + return self._planning_groups + def planning_scene_info(self) -> PlanningSceneInfo: """Return a stable metadata snapshot of the initialized planning scene.""" with self._lock: @@ -294,6 +314,46 @@ def get_current_joint_state(self, robot_id: WorldRobotID) -> JointState | None: ctx = self._world.get_live_context() return self._world.get_joint_state(ctx, robot_id) + def current_global_joint_state(self, max_age: float = 1.0) -> JointState: + """Return current state for all fresh robots with public global joint names.""" + names: list[str] = [] + positions: list[float] = [] + for robot_name, robot_id in self._robot_ids_by_name.items(): + if robot_id in self._state_monitors and self.is_state_stale(robot_id, max_age): + continue + state = self.get_current_joint_state(robot_id) + if state is None: + continue + for name, position in zip(state.name, state.position, strict=True): + names.append(f"{robot_name}/{name}") + positions.append(float(position)) + return JointState(name=names, position=positions) + + def current_group_joint_state( + self, group_id: PlanningGroupID, max_age: float = 1.0 + ) -> JointState: + """Return current joint state scoped and ordered for one planning group.""" + group = self._planning_groups.get(group_id) + robot_id = self._robot_ids_by_name[group.robot_name] + if robot_id in self._state_monitors and self.is_state_stale(robot_id, max_age): + raise ValueError(f"Current state for robot '{group.robot_name}' is stale") + state = self.get_current_joint_state(robot_id) + if state is None: + raise ValueError(f"Current state for robot '{group.robot_name}' is unavailable") + return filter_joint_state_to_selected_joints( + state, group.joint_names, group.local_joint_names + ) + + def _validate_planning_group_config(self, config: RobotModelConfig) -> None: + """Validate planning groups before mutating world/backend state.""" + seen_group_names: set[str] = set() + for definition in config.planning_groups: + group_id = make_planning_group_id(config.name, definition.name) + if definition.name in seen_group_names: + raise ValueError(f"Planning group '{group_id}' is already registered") + make_global_joint_names(config.name, definition.joint_names) + seen_group_names.add(definition.name) + def get_current_velocities(self, robot_id: WorldRobotID) -> JointState | None: """Get current joint velocities as JointState. Returns None if not available.""" if robot_id in self._state_monitors: @@ -367,15 +427,28 @@ 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.""" + robot_name = self._robot_configs[robot_id].name + group_id = self._planning_groups.primary_pose_group_id_for_robot(robot_name) + if group_id is None: + raise ValueError(f"Robot '{robot_name}' has no pose-targetable planning group") + return self.get_group_ee_pose(group_id, joint_state) + + def get_group_ee_pose( + self, group_id: PlanningGroupID, joint_state: JointState | None = None + ) -> PoseStamped: + """Get planning-group tip pose. Uses current robot state if joint_state is None.""" + group = self._planning_groups.get(group_id) + robot_id = self._robot_ids_by_name[group.robot_name] with self._world.scratch_context() as ctx: - # If no state provided, fetch current from state monitor if joint_state is None: + if robot_id in self._state_monitors and self.is_state_stale(robot_id): + raise ValueError(f"Current state for robot '{group.robot_name}' is stale") joint_state = self.get_current_joint_state(robot_id) + if joint_state is None: + raise ValueError(f"Current state for robot '{group.robot_name}' is unavailable") + self._world.set_joint_state(ctx, robot_id, joint_state) - 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_ee_pose(ctx, group_id) def get_link_pose( self, robot_id: WorldRobotID, link_name: str, joint_state: JointState | None = None @@ -411,9 +484,21 @@ def get_link_pose( def get_jacobian(self, robot_id: WorldRobotID, joint_state: JointState) -> NDArray[np.float64]: """Get 6xN Jacobian matrix.""" + robot_name = self._robot_configs[robot_id].name + group_id = self._planning_groups.primary_pose_group_id_for_robot(robot_name) + if group_id is None: + raise ValueError(f"Robot '{robot_name}' has no pose-targetable planning group") + return self.get_group_jacobian(group_id, joint_state) + + def get_group_jacobian( + self, group_id: PlanningGroupID, joint_state: JointState + ) -> NDArray[np.float64]: + """Get 6xN planning-group Jacobian matrix.""" + group = self._planning_groups.get(group_id) + robot_id = self._robot_ids_by_name[group.robot_name] 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) # Lifecycle diff --git a/dimos/manipulation/planning/spec/config.py b/dimos/manipulation/planning/spec/config.py index 74dc3bd69b..1ef40d103b 100644 --- a/dimos/manipulation/planning/spec/config.py +++ b/dimos/manipulation/planning/spec/config.py @@ -21,6 +21,11 @@ from pydantic import Field from dimos.core.module import ModuleConfig +from dimos.manipulation.planning.groups.identifiers import ( + assert_local_joint_names, + assert_valid_robot_name, +) +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -30,10 +35,13 @@ 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: Placement transform. This is the canonical world placement for + robot instances. + joint_names: Ordered list of controllable joints in the local model + namespace. This is not a planning group. + base_link: Robot-scoped link that base_pose places in the world and + current backends use for weld/placement. 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 +53,20 @@ 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. + joint_name_mapping: Maps coordinator joint names to local model joint names. + This is retained for current coordinator/monitor integrations while planning + APIs move toward globally scoped joint names. 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) joint_names: list[str] - end_effector_link: str 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 @@ -79,14 +88,44 @@ class RobotModelConfig(ModuleConfig): # Pre-grasp offset distance in meters (along approach direction) pre_grasp_offset: float = 0.10 + 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) + + @property + def end_effector_link(self) -> str: + """Compatibility pose target frame derived from planning groups. + + Current world, IK, and visualization layers still ask robot configs for + one end-effector link. The planning-group model stores that frame as a + group ``tip_link``; this shim keeps those layers working until they are + migrated to explicit planning-group IDs. + """ + pose_tip_links = [ + group.tip_link for group in self.planning_groups if group.tip_link is not None + ] + if not pose_tip_links: + raise ValueError( + f"RobotModelConfig '{self.name}' has no pose-target planning group; " + "define PlanningGroupDefinition.tip_link" + ) + unique_tip_links = list(dict.fromkeys(pose_tip_links)) + if len(unique_tip_links) > 1: + raise ValueError( + f"RobotModelConfig '{self.name}' has multiple pose-target planning groups; " + "use an explicit planning group ID" + ) + return unique_tip_links[0] + def get_urdf_joint_name(self, coordinator_name: str) -> str: - """Translate coordinator joint name to URDF joint name.""" + """Translate coordinator joint name to local model 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: + """Translate local model joint name to coordinator joint name.""" + for coord_name, model_name in self.joint_name_mapping.items(): + if model_name == urdf_name: return coord_name return urdf_name @@ -94,4 +133,4 @@ 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] + return [self.get_coordinator_joint_name(joint_name) for joint_name in self.joint_names] diff --git a/dimos/manipulation/planning/spec/models.py b/dimos/manipulation/planning/spec/models.py index d412e9f766..e746bf2bc2 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)""" diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index c7ee95ee0a..b5fea13a36 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -33,6 +33,7 @@ IKResult, JointPath, Obstacle, + PlanningGroupID, PlanningResult, PlanningSceneInfo, WorldRobotID, @@ -170,6 +171,14 @@ def get_jacobian(self, ctx: Any, robot_id: WorldRobotID) -> NDArray[np.float64]: """Get end-effector Jacobian (6 x n_joints).""" ... + def get_group_ee_pose(self, ctx: Any, group_id: PlanningGroupID) -> PoseStamped: + """Get planning-group tip pose.""" + ... + + def get_group_jacobian(self, ctx: Any, group_id: PlanningGroupID) -> NDArray[np.float64]: + """Get planning-group Jacobian (6 x n_group_joints).""" + ... + @runtime_checkable class VisualizationSpec(Protocol): diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index ca426ba340..dae0d3f73c 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -25,11 +25,18 @@ import numpy as np +from dimos.manipulation.planning.groups.identifiers import ( + make_global_joint_names, + make_planning_group_id, +) +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.groups.utils import joint_state_to_ordered_positions from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import ObstacleType from dimos.manipulation.planning.spec.models import ( JointPath, Obstacle, + PlanningGroupID, PlanningSceneInfo, WorldRobotID, ) @@ -207,6 +214,10 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: raise RuntimeError("Cannot add robot after world is finalized") with self._lock: + if any(data.config.name == config.name for data in self._robots.values()): + raise ValueError(f"Robot name '{config.name}' is already registered") + self._validate_planning_group_config(config) + self._robot_counter += 1 robot_id = f"robot_{self._robot_counter}" @@ -215,9 +226,16 @@ 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_link = config.base_link + try: + primary_group_id = self._primary_pose_group_id_for_config(config) + except ValueError: + primary_group_id = None + if primary_group_id is not None: + primary_group = self._planning_group_from_config(config, primary_group_id) + if primary_group.tip_link is not None: + ee_link = primary_group.tip_link + ee_frame = self._plant.GetBodyByName(ee_link, model_instance).body_frame() base_frame = self._plant.GetBodyByName(config.base_link, model_instance).body_frame() # Preview (yellow ghost) — always a separate instance per robot @@ -235,7 +253,6 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: base_frame=base_frame, preview_model_instance=preview_model_instance, ) - logger.info(f"Added robot '{robot_id}' ({config.name})") return robot_id @@ -317,6 +334,52 @@ def get_robot_config(self, robot_id: WorldRobotID) -> RobotModelConfig: raise KeyError(f"Robot '{robot_id}' not found") return self._robots[robot_id].config + @staticmethod + def _validate_planning_group_config(config: RobotModelConfig) -> None: + seen_group_names: set[str] = set() + for definition in config.planning_groups: + make_planning_group_id(config.name, definition.name) + if definition.name in seen_group_names: + raise ValueError(f"Planning group '{definition.name}' is already registered") + make_global_joint_names(config.name, definition.joint_names) + seen_group_names.add(definition.name) + + @staticmethod + def _planning_group_from_config( + config: RobotModelConfig, group_id: PlanningGroupID + ) -> PlanningGroup: + for definition in config.planning_groups: + if make_planning_group_id(config.name, definition.name) == group_id: + joint_names = tuple(make_global_joint_names(config.name, definition.joint_names)) + return PlanningGroup( + group_id, + config.name, + definition.name, + joint_names, + definition.joint_names, + definition.base_link, + definition.tip_link, + definition.source, + ) + raise KeyError(f"Unknown planning group ID: {group_id}") + + def _planning_group_from_id(self, group_id: PlanningGroupID) -> PlanningGroup: + for robot_data in self._robots.values(): + try: + return self._planning_group_from_config(robot_data.config, group_id) + except KeyError: + continue + raise KeyError(f"Unknown planning group ID: {group_id}") + + @staticmethod + def _primary_pose_group_id_for_config(config: RobotModelConfig) -> PlanningGroupID | None: + pose_groups = [group for group in config.planning_groups if group.has_pose_target] + if not pose_groups: + return None + if len(pose_groups) > 1: + raise ValueError(f"Robot '{config.name}' has multiple pose groups") + return make_planning_group_id(config.name, pose_groups[0].name) + def get_joint_limits( self, robot_id: WorldRobotID ) -> tuple[NDArray[np.float64], NDArray[np.float64]]: @@ -768,8 +831,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._joint_state_to_q(robot_id, joint_state) with self._lock: self._set_positions_internal(self._plant_context, robot_id, positions) @@ -787,8 +849,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._joint_state_to_q(robot_id, joint_state) # Get plant context from diagram context plant_ctx = self._diagram.GetMutableSubsystemContext(self._plant, ctx) @@ -809,6 +870,28 @@ def _set_positions_internal( self._plant.SetPositions(plant_ctx, full_positions) + def _joint_state_to_q( + self, robot_id: WorldRobotID, joint_state: JointState + ) -> NDArray[np.float64]: + """Normalize unnamed, robot-local, mapped, or global JointState to robot joint order.""" + if robot_id not in self._robots: + raise KeyError(f"Robot '{robot_id}' not found") + robot_data = self._robots[robot_id] + return joint_state_to_ordered_positions( + joint_state, + joint_names=robot_data.config.joint_names, + joint_name_mapping=robot_data.config.joint_name_mapping, + ) + + def _robot_id_for_group(self, group_id: PlanningGroupID) -> WorldRobotID: + group = self._planning_group_from_id(group_id) + matches = [ + rid for rid, data in self._robots.items() if data.config.name == group.robot_name + ] + if not matches: + raise KeyError(f"No robot registered for planning group '{group_id}'") + return matches[0] + def get_joint_state(self, ctx: Context, robot_id: WorldRobotID) -> JointState: """Get robot joint state from given context.""" if not self._finalized: @@ -905,16 +988,28 @@ def check_edge_collision_free( def get_ee_pose(self, ctx: Context, robot_id: WorldRobotID) -> PoseStamped: """Get end-effector pose.""" - if not self._finalized: - raise RuntimeError("World must be finalized first") - if robot_id not in self._robots: raise KeyError(f"Robot '{robot_id}' not found") - robot_data = self._robots[robot_id] + group_id = self._primary_pose_group_id_for_config(robot_data.config) + if group_id is None: + raise ValueError( + f"Robot '{robot_data.config.name}' has no pose-targetable planning group" + ) + return self.get_group_ee_pose(ctx, group_id) + + def get_group_ee_pose(self, ctx: Context, group_id: PlanningGroupID) -> PoseStamped: + """Get planning-group tip pose.""" + if not self._finalized: + raise RuntimeError("World must be finalized first") + + group = self._planning_group_from_id(group_id) + if group.tip_link is None: + raise ValueError(f"Planning group '{group_id}' has no tip link") + robot_data = self._robots[self._robot_id_for_group(group_id)] plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) - ee_body = robot_data.ee_frame.body() + ee_body = self._plant.GetBodyByName(group.tip_link, robot_data.model_instance) X_WE = self._plant.EvalBodyPoseInWorld(plant_ctx, ee_body) # Extract position and quaternion from Drake transform @@ -955,30 +1050,58 @@ def get_jacobian(self, ctx: Context, robot_id: WorldRobotID) -> NDArray[np.float Rows: [vx, vy, vz, wx, wy, wz] (linear, then angular) """ - if not self._finalized: - raise RuntimeError("World must be finalized first") - if robot_id not in self._robots: raise KeyError(f"Robot '{robot_id}' not found") - robot_data = self._robots[robot_id] + group_id = self._primary_pose_group_id_for_config(robot_data.config) + if group_id is None: + raise ValueError( + f"Robot '{robot_data.config.name}' has no pose-targetable planning group" + ) + return self.get_group_jacobian(ctx, group_id) + + def get_group_jacobian(self, ctx: Context, group_id: PlanningGroupID) -> NDArray[np.float64]: + """Get geometric Jacobian (6 x group joints) in group-local order.""" + if not self._finalized: + raise RuntimeError("World must be finalized first") + + group = self._planning_group_from_id(group_id) + if group.tip_link is None: + raise ValueError(f"Planning group '{group_id}' has no tip link") + robot_data = self._robots[self._robot_id_for_group(group_id)] plant_ctx = self._diagram.GetSubsystemContext(self._plant, ctx) + tip_frame = self._plant.GetBodyByName( + group.tip_link, robot_data.model_instance + ).body_frame() # Compute full Jacobian J_full = self._plant.CalcJacobianSpatialVelocity( plant_ctx, JacobianWrtVariable.kQDot, - robot_data.ee_frame, + tip_frame, np.array([0.0, 0.0, 0.0]), # type: ignore[arg-type] # Point on end-effector self._plant.world_frame(), self._plant.world_frame(), ) - # Extract columns for this robot's joints - n_joints = len(robot_data.joint_indices) + # Extract columns for configured controllable joints only. + joint_indices_by_name = dict( + zip(robot_data.config.joint_names, robot_data.joint_indices, strict=True) + ) + missing = [ + joint_name + for joint_name in group.local_joint_names + if joint_name not in joint_indices_by_name + ] + if missing: + raise ValueError( + f"Planning group '{group_id}' references non-controllable joints: {missing}" + ) + group_joint_indices = [joint_indices_by_name[name] for name in group.local_joint_names] + n_joints = len(group_joint_indices) J_robot = np.zeros((6, n_joints)) - for i, joint_idx in enumerate(robot_data.joint_indices): + for i, joint_idx in enumerate(group_joint_indices): J_robot[:, i] = J_full[:, joint_idx] # Reorder rows: Drake uses [angular, linear], we want [linear, angular] diff --git a/dimos/manipulation/planning/world/roboplan_world.py b/dimos/manipulation/planning/world/roboplan_world.py index d252d3fdc9..b5ac50d828 100644 --- a/dimos/manipulation/planning/world/roboplan_world.py +++ b/dimos/manipulation/planning/world/roboplan_world.py @@ -41,9 +41,20 @@ "Install the manipulation extra before selecting the roboplan backend." ) from exc +from dimos.manipulation.planning.groups.identifiers import ( + make_global_joint_names, + make_planning_group_id, +) +from dimos.manipulation.planning.groups.models import PlanningGroup +from dimos.manipulation.planning.groups.utils import joint_state_to_ordered_positions from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import ObstacleType, PlanningStatus -from dimos.manipulation.planning.spec.models import Obstacle, PlanningResult, WorldRobotID +from dimos.manipulation.planning.spec.models import ( + Obstacle, + PlanningGroupID, + PlanningResult, + WorldRobotID, +) from dimos.manipulation.planning.utils.mesh_utils import prepare_urdf_for_drake from dimos.manipulation.planning.utils.path_utils import compute_path_length from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -104,6 +115,9 @@ def add_robot(self, config: RobotModelConfig) -> WorldRobotID: raise ValueError("RoboPlanWorld currently supports one robot per Scene") if not Path(config.model_path).exists(): raise FileNotFoundError(f"Robot model not found: {Path(config.model_path).resolve()}") + if any(data.config.name == config.name for data in self._robots.values()): + raise ValueError(f"Robot name '{config.name}' is already registered") + self._validate_planning_group_config(config) self._validate_robot_config(config) self._robot_counter += 1 @@ -272,7 +286,17 @@ def check_edge_collision_free( def get_ee_pose(self, ctx: RoboPlanContext, robot_id: WorldRobotID) -> PoseStamped: """Get end-effector pose if RoboPlan exposes FK.""" robot = self._get_robot(robot_id) - mat = self.get_link_pose(ctx, robot_id, robot.config.end_effector_link) + group_id = self._primary_pose_group_id_for_config(robot.config) + if group_id is None: + raise ValueError(f"Robot '{robot.config.name}' has no pose-targetable planning group") + return self.get_group_ee_pose(ctx, group_id) + + def get_group_ee_pose(self, ctx: RoboPlanContext, group_id: PlanningGroupID) -> PoseStamped: + """Get planning-group tip pose if RoboPlan exposes FK.""" + group = self._planning_group_from_id(group_id) + if group.tip_link is None: + raise ValueError(f"Planning group '{group_id}' has no tip link") + mat = self.get_link_pose(ctx, self._robot_id_for_group(group_id), group.tip_link) pose = matrix_to_pose(mat) return PoseStamped( frame_id="world", @@ -299,17 +323,46 @@ def get_link_pose( def get_jacobian(self, ctx: RoboPlanContext, robot_id: WorldRobotID) -> NDArray[np.float64]: """Get end-effector Jacobian if RoboPlan exposes a compatible API.""" robot = self._get_robot(robot_id) + group_id = self._primary_pose_group_id_for_config(robot.config) + if group_id is None: + raise ValueError(f"Robot '{robot.config.name}' has no pose-targetable planning group") + return self.get_group_jacobian(ctx, group_id) + + def get_group_jacobian( + self, ctx: RoboPlanContext, group_id: PlanningGroupID + ) -> NDArray[np.float64]: + """Get planning-group Jacobian projected to group-local joint order.""" + group = self._planning_group_from_id(group_id) + if group.tip_link is None: + raise ValueError(f"Planning group '{group_id}' has no tip link") + robot_id = self._robot_id_for_group(group_id) + robot = self._get_robot(robot_id) q = ctx.q_by_robot.get(robot_id) if q is None: raise KeyError(f"Robot '{robot_id}' not found in context") scene = self._require_scene() - result = scene.computeFrameJacobian( - self._to_scene_q(robot_id, q), robot.config.end_effector_link, True - ) + result = scene.computeFrameJacobian(self._to_scene_q(robot_id, q), group.tip_link, True) arr = np.asarray(result, dtype=np.float64) if arr.shape[0] != 6: raise ValueError(f"Unexpected RoboPlan Jacobian shape: {arr.shape}; expected 6 x n") - return arr + scene_joint_order = self._query_scene_joint_order(scene, robot.config) + if scene_joint_order is not None and arr.shape[1] == len(scene_joint_order): + missing = [name for name in group.local_joint_names if name not in scene_joint_order] + if missing: + raise ValueError(f"Unknown joints for planning group '{group_id}': {missing}") + indices = [scene_joint_order.index(name) for name in group.local_joint_names] + return arr[:, indices] + if arr.shape[1] == len(robot.config.joint_names): + missing = [ + name for name in group.local_joint_names if name not in robot.config.joint_names + ] + if missing: + raise ValueError(f"Unknown joints for planning group '{group_id}': {missing}") + indices = [robot.config.joint_names.index(name) for name in group.local_joint_names] + return arr[:, indices] + raise ValueError( + f"Unexpected RoboPlan Jacobian shape: {arr.shape}; cannot project group '{group_id}'" + ) # PlannerSpec for native RoboPlan planning @@ -502,28 +555,78 @@ def _query_scene_joint_order( return None return list(group_info.joint_names) + def _validate_planning_group_config(self, config: RobotModelConfig) -> None: + """Validate planning groups before mutating backend state.""" + seen_group_names: set[str] = set() + for definition in config.planning_groups: + group_id = make_planning_group_id(config.name, definition.name) + if definition.name in seen_group_names: + raise ValueError(f"Planning group '{group_id}' is already registered") + make_global_joint_names(config.name, definition.joint_names) + seen_group_names.add(definition.name) + + def _planning_group_from_config( + self, config: RobotModelConfig, group_id: PlanningGroupID + ) -> PlanningGroup: + for definition in config.planning_groups: + if make_planning_group_id(config.name, definition.name) == group_id: + return 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, + ) + raise KeyError(f"Unknown planning group ID: {group_id}") + + def _planning_group_from_id(self, group_id: PlanningGroupID) -> PlanningGroup: + for robot in self._robots.values(): + try: + return self._planning_group_from_config(robot.config, group_id) + except KeyError: + continue + raise KeyError(f"Unknown planning group ID: {group_id}") + + def _primary_pose_group_id_for_config(self, config: RobotModelConfig) -> PlanningGroupID | None: + pose_group_ids = [ + make_planning_group_id(config.name, group.name) + for group in config.planning_groups + if group.has_pose_target + ] + if not pose_group_ids: + return None + if len(pose_group_ids) > 1: + raise ValueError( + f"Robot '{config.name}' has {len(pose_group_ids)} pose-targetable " + "planning groups; use an explicit planning group ID" + ) + return pose_group_ids[0] + def _get_robot(self, robot_id: WorldRobotID) -> _RoboPlanRobotData: if robot_id not in self._robots: raise KeyError(f"Robot '{robot_id}' not found") return self._robots[robot_id] + def _robot_id_for_group(self, group_id: PlanningGroupID) -> WorldRobotID: + group = self._planning_group_from_id(group_id) + matches = [ + rid for rid, data in self._robots.items() if data.config.name == group.robot_name + ] + if not matches: + raise KeyError(f"No robot registered for planning group '{group_id}'") + return matches[0] + def _joint_state_to_q( self, robot_id: WorldRobotID, joint_state: JointState ) -> NDArray[np.float64]: robot = self._get_robot(robot_id) - if len(joint_state.position) != len(robot.config.joint_names): - raise ValueError("JointState position length must match configured joint count") - if not joint_state.name: - return np.asarray(joint_state.position, dtype=np.float64) - name_to_pos = { - robot.config.get_urdf_joint_name(name): position - for name, position in zip(joint_state.name, joint_state.position, strict=True) - } - missing = [name for name in robot.config.joint_names if name not in name_to_pos] - if missing: - raise ValueError(f"JointState missing joints for RoboPlanWorld: {missing}") - return np.asarray( - [name_to_pos[name] for name in robot.config.joint_names], dtype=np.float64 + return joint_state_to_ordered_positions( + joint_state, + joint_names=robot.config.joint_names, + joint_name_mapping=robot.config.joint_name_mapping, ) def _require_finalized(self) -> None: 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..c981c76896 --- /dev/null +++ b/dimos/manipulation/planning/world/test_drake_world_planning_groups.py @@ -0,0 +1,195 @@ +# 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 pathlib import Path + +import numpy as np +import pytest + +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition +from dimos.manipulation.planning.spec.config import RobotModelConfig +from dimos.manipulation.planning.world.drake_world import DRAKE_AVAILABLE, DrakeWorld +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState + +requires_drake = pytest.mark.skipif( + not DRAKE_AVAILABLE, + reason="Drake planning-group tests require the manipulation extra", +) + + +def _write_urdf(path: Path) -> None: + path.write_text( + """ + + + + + + + + + + + + + + + +""" + ) + + +def _config( + path: Path, groups: list[PlanningGroupDefinition], joints: list[str] | None = None +) -> RobotModelConfig: + return RobotModelConfig( + name="arm", + model_path=path, + base_pose=PoseStamped(position=[0, 0, 0], orientation=[0, 0, 0, 1]), + joint_names=joints or ["joint1", "joint2"], + base_link="base_link", + planning_groups=groups, + ) + + +def _arm_group( + *joint_names: str, tip_link: str | None = "tool0", name: str = "arm" +) -> PlanningGroupDefinition: + return PlanningGroupDefinition( + name=name, joint_names=joint_names, base_link="base_link", tip_link=tip_link + ) + + +def test_drake_config_group_helpers_resolve_groups_without_drake_runtime(tmp_path: Path) -> None: + urdf = tmp_path / "robot.urdf" + _write_urdf(urdf) + config = _config(urdf, [_arm_group("joint2", "joint1", name="wrist")]) + + group = DrakeWorld._planning_group_from_config(config, "arm/wrist") + + assert DrakeWorld._primary_pose_group_id_for_config(config) == "arm/wrist" + assert group.id == "arm/wrist" + assert group.joint_names == ("arm/joint2", "arm/joint1") + assert group.local_joint_names == ("joint2", "joint1") + assert group.tip_link == "tool0" + + +def test_drake_config_group_helpers_validate_duplicate_and_ambiguous_groups( + tmp_path: Path, +) -> None: + urdf = tmp_path / "robot.urdf" + _write_urdf(urdf) + duplicate = _config( + urdf, + [_arm_group("joint1", name="same"), _arm_group("joint2", name="same")], + ) + ambiguous = _config( + urdf, + [_arm_group("joint1", name="a"), _arm_group("joint2", name="b")], + ) + + with pytest.raises(ValueError, match="already registered"): + DrakeWorld._validate_planning_group_config(duplicate) + with pytest.raises(ValueError, match="multiple pose"): + DrakeWorld._primary_pose_group_id_for_config(ambiguous) + with pytest.raises(KeyError, match="Unknown planning group ID"): + DrakeWorld._planning_group_from_config(ambiguous, "arm/missing") + + +@requires_drake +def test_drake_group_fk_uses_tip_link_and_legacy_unique_pose_group(tmp_path: Path) -> None: + urdf = tmp_path / "robot.urdf" + _write_urdf(urdf) + world = DrakeWorld() + robot_id = world.add_robot(_config(urdf, [_arm_group("joint1", "joint2")])) + world.finalize() + ctx = world.get_live_context() + world.set_joint_state( + ctx, robot_id, JointState({"name": ["joint1", "joint2"], "position": [0.0, 0.0]}) + ) + + group_pose = world.get_group_ee_pose(ctx, "arm/arm") + legacy_pose = world.get_ee_pose(ctx, robot_id) + + assert group_pose.position.x == pytest.approx(2.0) + assert legacy_pose.position.x == pytest.approx(group_pose.position.x) + assert world.get_jacobian(ctx, robot_id).shape == (6, 2) + + +@requires_drake +def test_drake_group_jacobian_shape_and_group_local_order(tmp_path: Path) -> None: + urdf = tmp_path / "robot.urdf" + _write_urdf(urdf) + world = DrakeWorld() + robot_id = world.add_robot( + _config( + urdf, + [ + _arm_group("joint1", "joint2", name="wrist_forward"), + _arm_group("joint2", "joint1", name="wrist_reverse"), + ], + ) + ) + world.finalize() + ctx = world.get_live_context() + world.set_joint_state( + ctx, robot_id, JointState({"name": ["joint1", "joint2"], "position": [0.0, 0.0]}) + ) + + forward_jacobian = world.get_group_jacobian(ctx, "arm/wrist_forward") + reverse_jacobian = world.get_group_jacobian(ctx, "arm/wrist_reverse") + + assert reverse_jacobian.shape == (6, 2) + np.testing.assert_allclose(reverse_jacobian[:, 0], forward_jacobian[:, 1]) + np.testing.assert_allclose(reverse_jacobian[:, 1], forward_jacobian[:, 0]) + + +@requires_drake +def test_drake_legacy_wrappers_fail_at_call_time_for_no_or_ambiguous_pose(tmp_path: Path) -> None: + urdf = tmp_path / "robot.urdf" + _write_urdf(urdf) + no_pose = DrakeWorld() + no_pose_id = no_pose.add_robot(_config(urdf, [_arm_group("joint1", tip_link=None)])) + no_pose.finalize() + with pytest.raises(ValueError, match="no pose-targetable"): + no_pose.get_ee_pose(no_pose.get_live_context(), no_pose_id) + + ambiguous = DrakeWorld() + ambiguous_id = ambiguous.add_robot( + _config( + urdf, + [ + _arm_group("joint1", tip_link="link1", name="a"), + _arm_group("joint2", tip_link="tool0", name="b"), + ], + ) + ) + ambiguous.finalize() + with pytest.raises(ValueError, match="multiple pose"): + ambiguous.get_jacobian(ambiguous.get_live_context(), ambiguous_id) + + +@requires_drake +def test_drake_group_jacobian_rejects_non_controllable_group_joints(tmp_path: Path) -> None: + urdf = tmp_path / "robot.urdf" + _write_urdf(urdf) + world = DrakeWorld() + world.add_robot(_config(urdf, [_arm_group("joint1", "joint2")], joints=["joint1"])) + world.finalize() + + with pytest.raises(ValueError, match="non-controllable"): + world.get_group_jacobian(world.get_live_context(), "arm/arm") diff --git a/dimos/manipulation/test_manipulation_module.py b/dimos/manipulation/test_manipulation_module.py index f8e914e3b3..e0958afb8c 100644 --- a/dimos/manipulation/test_manipulation_module.py +++ b/dimos/manipulation/test_manipulation_module.py @@ -30,6 +30,7 @@ ManipulationModule, ManipulationState, ) +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.msgs.geometry_msgs.Pose import Pose from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -62,8 +63,15 @@ def _get_xarm7_config() -> RobotModelConfig: model_path=desc_path / "urdf/xarm_device.urdf.xacro", base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), joint_names=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"], - end_effector_link="link7", base_link="link_base", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=("joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"), + base_link="link_base", + tip_link="link7", + ) + ], package_paths={"xarm_description": desc_path}, xacro_args={"dof": "7", "limited": "true"}, auto_convert_meshes=True, diff --git a/dimos/manipulation/test_manipulation_unit.py b/dimos/manipulation/test_manipulation_unit.py index bc12bfe994..c4adaf6212 100644 --- a/dimos/manipulation/test_manipulation_unit.py +++ b/dimos/manipulation/test_manipulation_unit.py @@ -28,6 +28,7 @@ ManipulationModuleConfig, ManipulationState, ) +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition 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 @@ -51,8 +52,15 @@ def robot_config(): model_path=Path("/path/to/robot.urdf"), base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), joint_names=["joint1", "joint2", "joint3"], - end_effector_link="link_tcp", base_link="link_base", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=("joint1", "joint2", "joint3"), + base_link="link_base", + tip_link="link_tcp", + ) + ], max_velocity=1.0, max_acceleration=2.0, coordinator_task_name="traj_arm", @@ -67,8 +75,15 @@ def robot_config_with_mapping(): model_path=Path("/path/to/robot.urdf"), base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), joint_names=["joint1", "joint2", "joint3"], - end_effector_link="link_tcp", base_link="link_base", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=("joint1", "joint2", "joint3"), + base_link="link_base", + tip_link="link_tcp", + ) + ], joint_name_mapping={ "left/joint1": "joint1", "left/joint2": "joint2", @@ -415,7 +430,11 @@ def test_execute_requires_task_name(self): model_path=Path("/path"), base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), joint_names=["j1"], - end_effector_link="ee", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", joint_names=("j1",), base_link="base_link", tip_link="ee" + ) + ], ) module._robots = {"arm": ("id", config_no_task, MagicMock())} module._planned_trajectories = {"arm": MagicMock()} @@ -598,8 +617,12 @@ def test_multi_robot_splits_correctly(self): model_path=Path("/path/to/robot.urdf"), base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), joint_names=["j1", "j2"], - end_effector_link="ee", base_link="base", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", joint_names=("j1", "j2"), base_link="base", tip_link="ee" + ) + ], joint_name_mapping={"left/j1": "j1", "left/j2": "j2"}, coordinator_task_name="traj_left", ) @@ -608,8 +631,12 @@ def test_multi_robot_splits_correctly(self): model_path=Path("/path/to/robot.urdf"), base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), joint_names=["j1", "j2"], - end_effector_link="ee", base_link="base", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", joint_names=("j1", "j2"), base_link="base", tip_link="ee" + ) + ], joint_name_mapping={"right/j1": "j1", "right/j2": "j2"}, coordinator_task_name="traj_right", ) diff --git a/dimos/manipulation/test_planning_factory.py b/dimos/manipulation/test_planning_factory.py index e695d9579f..99771f9fea 100644 --- a/dimos/manipulation/test_planning_factory.py +++ b/dimos/manipulation/test_planning_factory.py @@ -32,6 +32,7 @@ create_world, validate_backend_combination, ) +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.kinematics.config import JacobianKinematicsConfig from dimos.manipulation.planning.kinematics.jacobian_ik import JacobianIK from dimos.manipulation.planning.planners.rrt_planner import RRTConnectPlanner @@ -64,7 +65,14 @@ def robot_config() -> RobotModelConfig: model_path=Path("/path/to/robot.urdf"), base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] joint_names=["joint1", "joint2"], - end_effector_link="tcp", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=("joint1", "joint2"), + base_link="base_link", + tip_link="tcp", + ) + ], coordinator_task_name="traj_arm", ) diff --git a/dimos/manipulation/test_roboplan_world.py b/dimos/manipulation/test_roboplan_world.py index 9457c4264c..32eaa97bfa 100644 --- a/dimos/manipulation/test_roboplan_world.py +++ b/dimos/manipulation/test_roboplan_world.py @@ -25,6 +25,7 @@ import numpy as np import pytest +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.enums import ObstacleType, PlanningStatus from dimos.manipulation.planning.spec.models import Obstacle @@ -263,7 +264,14 @@ def robot_config(tmp_path: Path) -> RobotModelConfig: model_path=model_path, base_pose=PoseStamped(position=Vector3(), orientation=Quaternion()), # type: ignore[call-arg] joint_names=["joint1", "joint2"], - end_effector_link="tcp", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=("joint1", "joint2"), + base_link="base", + tip_link="tcp", + ) + ], joint_limits_lower=[-1.0, -2.0], joint_limits_upper=[1.0, 2.0], ) @@ -379,6 +387,37 @@ def test_joint_name_mapping_is_applied_to_input_states( assert live_round_trip.position == [0.2, 0.3] +def test_global_joint_names_are_mapped_without_regressing_coordinator_names( + fake_roboplan: None, robot_config: RobotModelConfig +) -> None: + robot_config.joint_name_mapping = {"arm/j1": "joint1", "arm/j2": "joint2"} + world, robot_id = _make_world(fake_roboplan, robot_config) + world.finalize() + + world.sync_from_joint_state( + robot_id, JointState(name=["arm/j1", "arm/j2"], position=[0.4, 0.5]) + ) + assert world.get_joint_state(world.get_live_context(), robot_id).position == [0.4, 0.5] + + world.sync_from_joint_state( + robot_id, JointState(name=["arm/joint1", "arm/joint2"], position=[0.2, 0.3]) + ) + assert world.get_joint_state(world.get_live_context(), robot_id).position == [0.2, 0.3] + + +def test_duplicate_resolved_joint_names_fail_clearly( + fake_roboplan: None, robot_config: RobotModelConfig +) -> None: + robot_config.joint_name_mapping = {"alias": "joint1"} + world, robot_id = _make_world(fake_roboplan, robot_config) + world.finalize() + + with pytest.raises(ValueError, match="duplicate joint 'joint1'"): + world.sync_from_joint_state( + robot_id, JointState(name=["joint1", "alias"], position=[0.1, 0.2]) + ) + + def test_obstacle_mutation_updates_scene_and_stored_pose( fake_roboplan: None, robot_config: RobotModelConfig ) -> None: @@ -465,6 +504,207 @@ def test_fk_jacobian_and_explicit_min_distance_unsupported( world.get_min_distance(ctx, robot_id) +def test_group_fk_and_jacobian_use_group_tip_and_local_joint_order( + fake_roboplan: None, robot_config: RobotModelConfig, monkeypatch: pytest.MonkeyPatch +) -> None: + config = robot_config.model_copy( + update={ + "joint_names": ["joint1", "joint2", "joint3"], + "planning_groups": [ + PlanningGroupDefinition( + name="wrist", + joint_names=("joint3", "joint1"), + base_link="base", + tip_link="wrist_tip", + ) + ], + "joint_limits_lower": [-1.0, -2.0, -3.0], + "joint_limits_upper": [1.0, 2.0, 3.0], + } + ) + monkeypatch.setattr(FakeScene, "joint_group_joint_names", ["joint2", "joint1", "joint3"]) + monkeypatch.setattr(FakeScene, "position_limits_lower", [-2.0, -1.0, -3.0]) + monkeypatch.setattr(FakeScene, "position_limits_upper", [2.0, 1.0, 3.0]) + fk_frames: list[str] = [] + + def fake_fk( + self: FakeScene, q: np.ndarray, frame_name: str, base_frame: str = "" + ) -> np.ndarray: + _ = (self, base_frame) + fk_frames.append(frame_name) + mat = np.eye(4) + mat[0, 3] = float(np.sum(q)) + return mat + + def fake_jacobian( + self: FakeScene, q: np.ndarray, frame_name: str, local: bool = True + ) -> np.ndarray: + _ = (self, q) + assert frame_name == "wrist_tip" + assert local is True + return np.arange(18, dtype=np.float64).reshape(6, 3) + + monkeypatch.setattr(FakeScene, "forwardKinematics", fake_fk) + monkeypatch.setattr(FakeScene, "computeFrameJacobian", fake_jacobian) + world, robot_id = _make_world(fake_roboplan, config) + world.finalize() + ctx = world.get_live_context() + world.set_joint_state( + ctx, + robot_id, + JointState({"name": ["joint1", "joint2", "joint3"], "position": [1.0, 2.0, 3.0]}), + ) + + pose = world.get_group_ee_pose(ctx, "arm/wrist") + jacobian = world.get_group_jacobian(ctx, "arm/wrist") + + assert fk_frames == ["wrist_tip"] + assert pose.position.x == pytest.approx(6.0) + np.testing.assert_allclose(jacobian, np.arange(18, dtype=np.float64).reshape(6, 3)[:, [2, 1]]) + + +def test_group_kinematics_reject_missing_tip_or_missing_context( + fake_roboplan: None, robot_config: RobotModelConfig +) -> None: + no_tip_config = robot_config.model_copy( + update={ + "planning_groups": [ + PlanningGroupDefinition( + name="joint_only", joint_names=("joint1", "joint2"), base_link="base" + ) + ] + } + ) + world, robot_id = _make_world(fake_roboplan, no_tip_config) + world.finalize() + + with pytest.raises(ValueError, match="no tip link"): + world.get_group_ee_pose(world.get_live_context(), "arm/joint_only") + with pytest.raises(ValueError, match="no tip link"): + world.get_group_jacobian(world.get_live_context(), "arm/joint_only") + + ctx = world.get_live_context() + del ctx.q_by_robot[robot_id] + with pytest.raises(KeyError, match=robot_id): + world.get_link_pose(ctx, robot_id, "tcp") + + jacobian_world, jacobian_robot_id = _make_world(fake_roboplan, robot_config) + jacobian_world.finalize() + jacobian_ctx = jacobian_world.get_live_context() + del jacobian_ctx.q_by_robot[jacobian_robot_id] + with pytest.raises(KeyError, match=jacobian_robot_id): + jacobian_world.get_group_jacobian(jacobian_ctx, "arm/manipulator") + + +def test_group_jacobian_validates_projection_shape_and_joint_names( + fake_roboplan: None, + robot_config: RobotModelConfig, + monkeypatch: pytest.MonkeyPatch, +) -> None: + world, robot_id = _make_world(fake_roboplan, robot_config) + world.finalize() + ctx = world.get_live_context() + world.set_joint_state(ctx, robot_id, JointState(name=["joint1", "joint2"], position=[0.0, 0.0])) + + monkeypatch.setattr(FakeScene, "joint_group_joint_names", ["joint1", "other"]) + monkeypatch.setattr( + FakeScene, + "computeFrameJacobian", + lambda self, q, frame_name, local=True: np.ones((6, 2)), + ) + with pytest.raises(ValueError, match="Unknown joints"): + world.get_group_jacobian(ctx, "arm/manipulator") + + monkeypatch.setattr(FakeScene, "joint_group_joint_names", ["joint1", "joint2"]) + monkeypatch.setattr( + FakeScene, + "computeFrameJacobian", + lambda self, q, frame_name, local=True: np.ones((5, 2)), + ) + with pytest.raises(ValueError, match="Unexpected RoboPlan Jacobian shape"): + world.get_group_jacobian(ctx, "arm/manipulator") + + monkeypatch.setattr( + FakeScene, + "computeFrameJacobian", + lambda self, q, frame_name, local=True: np.ones((6, 3)), + ) + with pytest.raises(ValueError, match="cannot project"): + world.get_group_jacobian(ctx, "arm/manipulator") + + +def test_group_jacobian_falls_back_to_configured_joint_order_when_scene_order_is_unavailable( + fake_roboplan: None, + robot_config: RobotModelConfig, + monkeypatch: pytest.MonkeyPatch, +) -> None: + world, robot_id = _make_world(fake_roboplan, robot_config) + world.finalize() + ctx = world.get_live_context() + world.set_joint_state(ctx, robot_id, JointState(name=["joint1", "joint2"], position=[0.0, 0.0])) + + def missing_group_info(self: FakeScene, name: str) -> FakeJointGroupInfo: + _ = (self, name) + raise AttributeError("no joint group info") + + monkeypatch.setattr(FakeScene, "getJointGroupInfo", missing_group_info) + monkeypatch.setattr( + FakeScene, + "computeFrameJacobian", + lambda self, q, frame_name, local=True: np.arange(12, dtype=np.float64).reshape(6, 2), + ) + + np.testing.assert_allclose( + world.get_group_jacobian(ctx, "arm/manipulator"), + np.arange(12, dtype=np.float64).reshape(6, 2), + ) + + +def test_legacy_kinematics_wrappers_require_unique_pose_group( + fake_roboplan: None, robot_config: RobotModelConfig +) -> None: + no_pose_config = robot_config.model_copy( + update={ + "planning_groups": [ + PlanningGroupDefinition(name="base", joint_names=("joint1",), base_link="base") + ] + } + ) + no_pose_world, no_pose_id = _make_world(fake_roboplan, no_pose_config) + no_pose_world.finalize() + with pytest.raises(ValueError, match="no pose-targetable"): + no_pose_world.get_ee_pose(no_pose_world.get_live_context(), no_pose_id) + with pytest.raises(ValueError, match="no pose-targetable"): + no_pose_world.get_jacobian(no_pose_world.get_live_context(), no_pose_id) + + ambiguous_config = robot_config.model_copy( + update={ + "planning_groups": [ + PlanningGroupDefinition( + name="a", joint_names=("joint1",), base_link="base", tip_link="a_tip" + ), + PlanningGroupDefinition( + name="b", joint_names=("joint2",), base_link="base", tip_link="b_tip" + ), + ] + } + ) + ambiguous_world, ambiguous_id = _make_world(fake_roboplan, ambiguous_config) + ambiguous_world.finalize() + with pytest.raises(ValueError, match="pose-targetable planning groups"): + ambiguous_world.get_jacobian(ambiguous_world.get_live_context(), ambiguous_id) + + +def test_group_lookup_rejects_unknown_group_id( + fake_roboplan: None, robot_config: RobotModelConfig +) -> None: + world, _ = _make_world(fake_roboplan, robot_config) + world.finalize() + + with pytest.raises(KeyError, match="Unknown planning group ID"): + world.get_group_ee_pose(world.get_live_context(), "other/missing") + + def test_native_planner_converts_path(fake_roboplan: None, robot_config: RobotModelConfig) -> None: world, robot_id = _make_world(fake_roboplan, robot_config) world.finalize() diff --git a/dimos/manipulation/visualization/test_factory.py b/dimos/manipulation/visualization/test_factory.py index 4d49f019cf..0ad698748d 100644 --- a/dimos/manipulation/visualization/test_factory.py +++ b/dimos/manipulation/visualization/test_factory.py @@ -24,6 +24,7 @@ import pytest from dimos.manipulation.manipulation_module import ManipulationModuleConfig +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import ( JointPath, @@ -78,7 +79,11 @@ def get_robot_config(self, robot_id: WorldRobotID) -> RobotModelConfig: model_path=Path("fake.urdf"), base_pose=PoseStamped(), joint_names=[], - end_effector_link="ee_link", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", joint_names=(), base_link="base_link", tip_link="ee_link" + ) + ], ) def get_joint_limits( diff --git a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py index 19f1f2da60..63891bad25 100644 --- a/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py +++ b/dimos/manipulation/visualization/viser/test_visualizer_lifecycle.py @@ -21,6 +21,7 @@ pytest.importorskip("viser", reason="Viser optional dependency is not installed") +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.manipulation.planning.spec.models import PlanningSceneInfo from dimos.manipulation.visualization.viser import ( @@ -63,7 +64,11 @@ def fake_robot_config(name: str) -> RobotModelConfig: model_path=Path(f"{name}.urdf"), base_pose=PoseStamped(), joint_names=[], - end_effector_link="ee_link", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", joint_names=(), base_link="base_link", tip_link="ee_link" + ) + ], ) diff --git a/dimos/robot/manipulators/_modeling.py b/dimos/robot/manipulators/_modeling.py index 852a484b63..d9b528d0d5 100644 --- a/dimos/robot/manipulators/_modeling.py +++ b/dimos/robot/manipulators/_modeling.py @@ -16,6 +16,7 @@ from __future__ import annotations +import math from typing import TypeAlias from dimos.manipulation.planning.spec.models import RobotName @@ -31,10 +32,16 @@ JointNameMapping: TypeAlias = dict[CoordinatorJointName, UrdfJointName] -def base_pose(x: float = 0.0, y: float = 0.0, z: float = 0.0) -> PoseStamped: +def base_pose( + x: float = 0.0, + y: float = 0.0, + z: float = 0.0, + pitch: float = 0.0, +) -> PoseStamped: + half_pitch = pitch / 2.0 return PoseStamped( position=Vector3(x=x, y=y, z=z), - orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + orientation=Quaternion([0.0, math.sin(half_pitch), 0.0, math.cos(half_pitch)]), ) diff --git a/dimos/robot/manipulators/a750/config.py b/dimos/robot/manipulators/a750/config.py index 32bc57409a..4711c2f537 100644 --- a/dimos/robot/manipulators/a750/config.py +++ b/dimos/robot/manipulators/a750/config.py @@ -21,6 +21,7 @@ from dimos.control.components import HardwareComponent, HardwareType, make_joints from dimos.core.global_config import global_config +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.robot.manipulators._modeling import ( base_pose, @@ -102,13 +103,21 @@ def make_a750_model_config( coordinator_task_name: str | None = None, ) -> RobotModelConfig: dof = 6 + local_joint_names = joint_names(dof) return RobotModelConfig( name=name, model_path=A750_MODEL_PATH, base_pose=base_pose(), - joint_names=joint_names(dof), - end_effector_link="gripper_base", + joint_names=local_joint_names, base_link="base_link", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=tuple(local_joint_names), + base_link="base_link", + tip_link="gripper_base", + ) + ], package_paths=A750_PACKAGE_PATHS, auto_convert_meshes=True, collision_exclusion_pairs=A750_GRIPPER_COLLISION_EXCLUSIONS, diff --git a/dimos/robot/manipulators/openarm/config.py b/dimos/robot/manipulators/openarm/config.py index 5306d408bf..45b935b336 100644 --- a/dimos/robot/manipulators/openarm/config.py +++ b/dimos/robot/manipulators/openarm/config.py @@ -20,6 +20,7 @@ from typing import Any from dimos.control.components import HardwareComponent, HardwareType +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.robot.manipulators._modeling import base_pose from dimos.utils.data import LfsPath @@ -81,13 +82,21 @@ def openarm_hardware( def openarm_model_config(side: str, name: str | None = None) -> RobotModelConfig: validate_side(side) resolved_name = name or f"{side}_arm" + local_joint_names = openarm_joints(side) return RobotModelConfig( name=resolved_name, model_path=OPENARM_LEFT_MODEL if side == "left" else OPENARM_RIGHT_MODEL, base_pose=base_pose(), - joint_names=openarm_joints(side), - end_effector_link=f"openarm_{side}_link7", + joint_names=local_joint_names, base_link="openarm_body_link0", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=tuple(local_joint_names), + base_link="openarm_body_link0", + tip_link=f"openarm_{side}_link7", + ) + ], package_paths=OPENARM_PACKAGE_PATHS, collision_exclusion_pairs=OPENARM_COLLISION_EXCLUSIONS, auto_convert_meshes=True, @@ -112,13 +121,21 @@ def openarm_single_hardware( def openarm_single_model_config() -> RobotModelConfig: + local_joint_names = openarm_joints("left") return RobotModelConfig( name="arm", model_path=OPENARM_V10_FK_MODEL, base_pose=base_pose(), - joint_names=openarm_joints("left"), - end_effector_link="openarm_left_link7", + joint_names=local_joint_names, base_link="openarm_body_link0", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=tuple(local_joint_names), + base_link="openarm_body_link0", + tip_link="openarm_left_link7", + ) + ], package_paths=OPENARM_PACKAGE_PATHS, auto_convert_meshes=True, max_velocity=0.5, diff --git a/dimos/robot/manipulators/piper/config.py b/dimos/robot/manipulators/piper/config.py index f99e9589a5..68772166ad 100644 --- a/dimos/robot/manipulators/piper/config.py +++ b/dimos/robot/manipulators/piper/config.py @@ -20,6 +20,7 @@ from dimos.control.components import HardwareComponent, HardwareType, make_joints from dimos.core.global_config import global_config +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.robot.manipulators._modeling import ( base_pose, @@ -110,13 +111,21 @@ def make_piper_model_config( home_joints: list[float] | None = None, ) -> RobotModelConfig: dof = 6 + local_joint_names = joint_names(dof) return RobotModelConfig( name=name, model_path=PIPER_MODEL_PATH, base_pose=base_pose(), - joint_names=joint_names(dof), - end_effector_link="gripper_base", + joint_names=local_joint_names, base_link="base_link", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=tuple(local_joint_names), + base_link="base_link", + tip_link="gripper_base", + ) + ], package_paths=PIPER_PACKAGE_PATHS, auto_convert_meshes=True, collision_exclusion_pairs=PIPER_GRIPPER_COLLISION_EXCLUSIONS, diff --git a/dimos/robot/manipulators/xarm/config.py b/dimos/robot/manipulators/xarm/config.py index d918ee94a8..3f6beb7afc 100644 --- a/dimos/robot/manipulators/xarm/config.py +++ b/dimos/robot/manipulators/xarm/config.py @@ -21,6 +21,7 @@ from dimos.control.components import HardwareComponent, HardwareType, make_joints from dimos.core.global_config import global_config +from dimos.manipulation.planning.groups.models import PlanningGroupDefinition from dimos.manipulation.planning.spec.config import RobotModelConfig from dimos.robot.manipulators._modeling import ( base_pose, @@ -164,19 +165,28 @@ def make_xarm_model_config( xacro_args = { "dof": str(dof), "limited": "true", - "attach_xyz": f"{x_offset} {y_offset} {z_offset}", - "attach_rpy": f"0 {pitch} 0", + "attach_xyz": "0 0 0", + "attach_rpy": "0 0 0", } if add_gripper: xacro_args["add_gripper"] = "true" + local_joint_names = joint_names(dof) + tip_link = "link_tcp" if add_gripper else f"link{dof}" return RobotModelConfig( name=name, model_path=XARM_MODEL_PATH, - base_pose=base_pose(x_offset, y_offset, z_offset), - joint_names=joint_names(dof), - end_effector_link="link_tcp" if add_gripper else f"link{dof}", + base_pose=base_pose(x_offset, y_offset, z_offset, pitch), + joint_names=local_joint_names, base_link="link_base", + planning_groups=[ + PlanningGroupDefinition( + name="manipulator", + joint_names=tuple(local_joint_names), + base_link="link_base", + tip_link=tip_link, + ) + ], package_paths=XARM_PACKAGE_PATHS, xacro_args=xacro_args, auto_convert_meshes=True, diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index aaa075ee49..b173ff22d1 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,31 @@ 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", + ) + ], + base_pose=_make_base_pose(y=y_offset), # world -> base_link placement + base_link="base_link", # Robot-scoped placement/weld/strip link 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 +550,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 +564,23 @@ 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; direct `RobotModelConfig(...)` helpers should pass explicit groups, while shared config helpers can discover groups from SRDF/fallback | +| `base_pose` / `base_link` | Optional robot placement: `base_pose` places `base_link` in the world for weld/strip behavior | | `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. + +Planning-group `base_link`/`tip_link` values define kinematic chains and pose +target frames. `base_link` is only the robot-scoped link placed by +`base_pose`; do not use it as a substitute for planning-group chain metadata. +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..043899fd50 --- /dev/null +++ b/docs/capabilities/manipulation/planning_groups.md @@ -0,0 +1,161 @@ +# Manipulation Planning Groups + +Planning groups are named, selectable kinematic chains used by manipulation +planning. They let APIs target a specific part of a robot, such as an arm or +torso, without confusing that group with the robot's hardware identity. + +## Concepts + +| Concept | Meaning | +|---------|---------| +| Robot name | The configured robot ID in `RobotModelConfig.name`. | +| Planning group | A named serial chain of controllable joints on one robot. | +| Planning group ID | Stable API ID in the form `{robot_name}/{group_name}`. | +| Local joint name | Joint name inside a robot model, such as `joint1`. | +| Global joint name | Boundary-level joint name in the form `{robot_name}/{local_joint_name}`. | +| Generated plan | Planning artifact containing selected group IDs and one synchronized global-joint path. | +| Auxiliary group | A selected group that contributes free DOFs to a pose plan without receiving its own pose target. | + +Local URDF/SRDF joint names stay inside robot-scoped configuration, model +parsing, and backend internals. Flat planning states and generated plan paths +use global joint names so multiple robots can safely share local names such as +`joint1`. + +## Discovering planning groups + +Planning-group config helpers can discover groups in this order: + +1. Explicit `planning_groups` on the robot model config. +2. Explicit `srdf_path` on the robot model config. +3. Conservative SRDF auto-discovery near the model path, with a warning. +4. Fallback generation of one `{robot_name}/manipulator` group when the + configured controllable joints form exactly one unambiguous serial chain. +5. Error if no SRDF or fallback chain can provide a single valid group. + +Direct `RobotModelConfig(...)` construction does not run discovery or synthesize +groups in `model_post_init`; callers must pass explicit `planning_groups` there. + +Supported SRDF group forms: + +```xml + + + +``` + +```xml + + + + + +``` + +Unsupported SRDF forms are skipped with warnings: link groups, nested group +references, mixed group declarations, branching or non-serial groups, and SRDF +`` metadata. A chain group's `tip_link` is its pose target frame. +An ordered joint-list group can be pose-targeted only when DimOS can validate a +unique serial target frame. + +## Fallback behavior + +When no SRDF or explicit group config 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`. + +## Current APIs + +Use `list_planning_groups()` to discover group IDs and capabilities before +planning: + +```python skip +groups = manip.list_planning_groups() +pose_groups = [group for group in groups if group.has_pose_target] +group_id = pose_groups[0].id +``` + +Joint-space planning targets group IDs and uses local joint names inside each +target `JointState`: + +```python skip +ok = manip.plan_to_joint_targets( + { + "left_arm/manipulator": JointState( + name=["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"], + position=[0.0, -0.4, 0.2, 0.0, 0.3, 0.0], + ) + } +) +``` + +Pose planning targets pose-capable group IDs. Add auxiliary groups when another +chain should participate as free DOFs but does not have its own pose target: + +```python skip +ok = manip.plan_to_pose_targets( + {"left_arm/manipulator": target_pose}, + auxiliary_groups=["torso/manipulator"], +) +``` + +After a successful planning call, preview and execution use the module's current +stored plan: + +```python skip +manip.preview_plan() +manip.execute_plan() +``` + +Callers that already hold a `GeneratedPlan` may pass it explicitly: + +```python skip +manip.preview_plan(plan) +manip.execute_plan(plan) +``` + +For robot-scoped compatibility APIs, unnamed joint vectors are interpreted in +the robot model's configured 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: if one trajectory task accepts and a later +task rejects, DimOS reports the rejection but does not roll back the accepted +task. + +## Robot placement config + +`RobotModelConfig.base_pose` and `RobotModelConfig.base_link` describe robot +placement: `base_pose` places `base_link` in the world and current backends +use that link for weld/placement and optional model-authored world-joint +stripping. This is robot placement metadata, not planning-chain metadata. + +Planning-group `base_link` and `tip_link` values are the only source for chain +bases and pose target frames. Robot-scoped end-effector config is no longer +supported; robot-level EE helper APIs are wrappers over a unique pose-targetable +planning group and should use explicit group APIs when multiple pose groups +exist. + +Robot placement can be encoded either in model assets or in `base_pose`, +depending on the blueprint. `joint_names` remains supported and should describe +the ordered controllable local model joint set. diff --git a/openspec/changes/control-docs-housekeeping/.openspec.yaml b/openspec/changes/control-docs-housekeeping/.openspec.yaml new file mode 100644 index 0000000000..34f9314d22 --- /dev/null +++ b/openspec/changes/control-docs-housekeeping/.openspec.yaml @@ -0,0 +1,2 @@ +schema: spec-driven +created: 2026-06-29 diff --git a/openspec/changes/control-docs-housekeeping/README.md b/openspec/changes/control-docs-housekeeping/README.md new file mode 100644 index 0000000000..ee333ffc5a --- /dev/null +++ b/openspec/changes/control-docs-housekeeping/README.md @@ -0,0 +1,3 @@ +# control-docs-housekeeping + +Extract PR 6 from reference branch cc/spec/movegroup: peel off control/coordinator/generated/docs/housekeeping changes not central to planning-group semantics. diff --git a/openspec/changes/control-docs-housekeeping/design.md b/openspec/changes/control-docs-housekeeping/design.md new file mode 100644 index 0000000000..c42efe8508 --- /dev/null +++ b/openspec/changes/control-docs-housekeeping/design.md @@ -0,0 +1,26 @@ +## Context + +These files do not obviously belong to the core planning-group review. Some may be genuine integration requirements; others may be incidental edits from the large branch. + +## Goals / Non-Goals + +**Goals:** +- Separate orthogonal changes from the planning-group PR stack. +- Keep generated files and docs synchronized only when required. +- Provide a clear report of accidental or questionable files. + +**Non-Goals:** +- Do not change planning-group semantics. +- Do not include Viser UI or planner algorithm changes. +- Do not include files just because they exist on the reference branch. + +## Decisions + +- Treat this change as a triage/extraction PR, not a mandatory feature PR. +- Include control/task changes only if validation shows they are required or independently useful. +- Keep generated registry changes next to the PR that introduces the corresponding blueprint, unless generation must happen at the end of the stack. + +## Risks / Trade-offs + +- If control changes depend on module API changes, this PR should stack after PR 4. +- If docs describe earlier PR behavior, those docs should move into the earlier PRs instead of this housekeeping PR. diff --git a/openspec/changes/control-docs-housekeeping/proposal.md b/openspec/changes/control-docs-housekeeping/proposal.md new file mode 100644 index 0000000000..436f94fb41 --- /dev/null +++ b/openspec/changes/control-docs-housekeeping/proposal.md @@ -0,0 +1,25 @@ +## Why + +The current branch contains small control, generated-registry, docs, and housekeeping changes mixed into the planning-group refactor. These should be peeled off so reviewers can decide whether they are required or accidental. + +## What Changes + +- Extract control/coordinator task and tick-loop changes if they are truly required. +- Extract generated blueprint registry updates if they are caused by earlier PRs. +- Extract remaining docs/readme cleanup and packaging/config touches. +- Report any file that appears accidental rather than including it blindly. + +## Capabilities + +### New Capabilities +- `control-docs-housekeeping`: Distribution plan for orthogonal control, generated, docs, and repo housekeeping changes. + +### Modified Capabilities +- `control-coordinator-integration`: Control/coordinator behavior updates only if validated as required by the planning-group stack. + +## Impact + +- Base branch: preferably PR 4 `manipulation-module-group-api`, or `main` for truly independent control/docs changes. +- Reference implementation: `cc/spec/movegroup`. +- Primary candidate files: `dimos/control/*`, `dimos/e2e_tests/test_control_coordinator.py`, generated blueprint files, docs/readmes, `pyproject.toml`, `AGENTS.md`. +- Out of scope: planning-group core, planners, module API, and Viser UI. diff --git a/openspec/changes/control-docs-housekeeping/specs/control-docs-housekeeping/spec.md b/openspec/changes/control-docs-housekeeping/specs/control-docs-housekeeping/spec.md new file mode 100644 index 0000000000..cc200026e1 --- /dev/null +++ b/openspec/changes/control-docs-housekeeping/specs/control-docs-housekeeping/spec.md @@ -0,0 +1,15 @@ +## ADDED Requirements + +### Requirement: Orthogonal changes must be reviewed separately +Changes that are not necessary for planning-group semantics MUST be extracted into a separate review or explicitly excluded from the stack. + +#### Scenario: Candidate file is unrelated to planning groups +- **WHEN** a file change is docs-only, generated-only, control-only, or repo-housekeeping-only +- **THEN** it is reviewed in this housekeeping change or reported as intentionally excluded + +### Requirement: Generated files must match their source changes +Generated registry files MUST be included only with the source blueprint/config change that requires regeneration, or in a final generated-files PR. + +#### Scenario: Blueprint registry changes are generated +- **WHEN** a generated registry file differs from `main` +- **THEN** the extraction task identifies the source change and validates regeneration diff --git a/openspec/changes/control-docs-housekeeping/tasks.md b/openspec/changes/control-docs-housekeeping/tasks.md new file mode 100644 index 0000000000..6831183321 --- /dev/null +++ b/openspec/changes/control-docs-housekeeping/tasks.md @@ -0,0 +1,17 @@ +## 1. Triage candidate files + +- [ ] 1.1 Compare each candidate file against the reference branch and decide whether it is required, independent, generated, docs-only, or accidental. +- [ ] 1.2 Extract only required or intentionally independent files. +- [ ] 1.3 Move docs back into earlier PRs when they explain earlier PR behavior. + +## 2. Control/generated/docs extraction + +- [ ] 2.1 Extract control/task changes if they are intentional. +- [ ] 2.2 Extract generated blueprint registry changes only when their source blueprint is included. +- [ ] 2.3 Extract `pyproject.toml` and `AGENTS.md` only if justified; otherwise report as excluded. + +## 3. Validation + +- [ ] 3.1 Run `uv run pytest dimos/control/test_control.py dimos/e2e_tests/test_control_coordinator.py dimos/robot/test_all_blueprints.py -q` for included files. +- [ ] 3.2 Run any docs or generated-registry checks required by included changes. +- [ ] 3.3 Return a file-by-file inclusion/exclusion summary. diff --git a/openspec/changes/group-aware-ik-rrt/.openspec.yaml b/openspec/changes/group-aware-ik-rrt/.openspec.yaml new file mode 100644 index 0000000000..34f9314d22 --- /dev/null +++ b/openspec/changes/group-aware-ik-rrt/.openspec.yaml @@ -0,0 +1,2 @@ +schema: spec-driven +created: 2026-06-29 diff --git a/openspec/changes/group-aware-ik-rrt/README.md b/openspec/changes/group-aware-ik-rrt/README.md new file mode 100644 index 0000000000..872d917e36 --- /dev/null +++ b/openspec/changes/group-aware-ik-rrt/README.md @@ -0,0 +1,3 @@ +# group-aware-ik-rrt + +Extract PR 3 from reference branch cc/spec/movegroup: make IK and RRT consume explicit planning groups and group-local targets. diff --git a/openspec/changes/group-aware-ik-rrt/design.md b/openspec/changes/group-aware-ik-rrt/design.md new file mode 100644 index 0000000000..6187e24bb9 --- /dev/null +++ b/openspec/changes/group-aware-ik-rrt/design.md @@ -0,0 +1,27 @@ +## Context + +IK and RRT previously operated mostly through robot-scoped assumptions. With groups, each target must map to an explicit group, local joint order, and target frame. + +## Goals / Non-Goals + +**Goals:** +- Make IK target frames come from the requested planning group. +- Make RRT accept and return group-local joint targets and paths. +- Keep collision checks and global robot-state projection correct. +- Fail clearly when a requested group lacks required pose target metadata. + +**Non-Goals:** +- Do not change `ManipulationModule` public APIs in this PR. +- Do not include visualization changes. +- Do not change control tasks. + +## Decisions + +- Prefer explicit group IDs for all new solver/planner paths. +- Robot-scoped compatibility may resolve through a unique pose-targetable group, but ambiguous robots must fail clearly. +- PinkIK and Viser-style base-pose transforms must validate that robot-scoped `base_link` is compatible with model-root assumptions. + +## Risks / Trade-offs + +- Algorithm tests can become broad quickly. Keep this PR focused on solver/planner contracts and leave module-level behavior to PR 4. +- Some optional dependencies may be unavailable locally; fake dependency tests should stay hermetic. diff --git a/openspec/changes/group-aware-ik-rrt/proposal.md b/openspec/changes/group-aware-ik-rrt/proposal.md new file mode 100644 index 0000000000..0ea8cb9a14 --- /dev/null +++ b/openspec/changes/group-aware-ik-rrt/proposal.md @@ -0,0 +1,24 @@ +## Why + +After world backends understand planning groups, the solver and planner layer must consume group IDs and group-local targets. This PR keeps algorithm review separate from both backend plumbing and public module/UI migration. + +## What Changes + +- Update PinkIK, Jacobian IK, and Drake optimization IK to resolve target frames from planning groups. +- Update RRT planning to handle group-local joint targets, group candidate selection, collision checking, and result projection. +- Add focused IK/planner tests. + +## Capabilities + +### New Capabilities +- `group-aware-ik-rrt`: IK and RRT planning support explicit planning groups and group-local targets. + +### Modified Capabilities +- `manipulation-planning-algorithms`: Planners and IK solvers no longer rely on robot-scoped end-effector metadata. + +## Impact + +- Base branch: PR 2 `planning-world-monitor-groups`. +- Reference implementation: `cc/spec/movegroup`. +- Primary files: `dimos/manipulation/planning/kinematics/*`, `dimos/manipulation/planning/planners/rrt_planner.py`, related tests. +- Out of scope: public `ManipulationModule` API migration and Viser UI. diff --git a/openspec/changes/group-aware-ik-rrt/specs/group-aware-ik-rrt/spec.md b/openspec/changes/group-aware-ik-rrt/specs/group-aware-ik-rrt/spec.md new file mode 100644 index 0000000000..8d477a3035 --- /dev/null +++ b/openspec/changes/group-aware-ik-rrt/specs/group-aware-ik-rrt/spec.md @@ -0,0 +1,22 @@ +## ADDED Requirements + +### Requirement: IK solvers must use planning-group target frames +IK solvers MUST resolve pose target frames from the requested planning group's `tip_link` rather than from robot-scoped end-effector metadata. + +#### Scenario: Pose IK targets a manipulator group +- **WHEN** a pose target is submitted for a group with `tip_link="tcp"` +- **THEN** the IK solver constrains the `tcp` frame + +### Requirement: Planners must preserve group-local joint ordering +Planners MUST accept and return joint targets in the requested group's local joint order while projecting through full robot state for collision checks. + +#### Scenario: Group joint target uses subset order +- **WHEN** a group target names a subset of robot joints +- **THEN** planning uses the correct full robot state and returns a path scoped to the group + +### Requirement: Algorithms must fail clearly for non-pose-targetable groups +Algorithms that require a pose target frame MUST return an explicit failure when the requested group has no `tip_link`. + +#### Scenario: Pose IK targets a joint-only group +- **WHEN** a pose target is submitted for a group without `tip_link` +- **THEN** the solver returns a no-solution or unsupported result with an explanatory message diff --git a/openspec/changes/group-aware-ik-rrt/tasks.md b/openspec/changes/group-aware-ik-rrt/tasks.md new file mode 100644 index 0000000000..3a13b0a9f7 --- /dev/null +++ b/openspec/changes/group-aware-ik-rrt/tasks.md @@ -0,0 +1,19 @@ +## 1. Extract algorithm changes + +- [ ] 1.1 Start from PR 2 and use `cc/spec/movegroup` as reference. +- [ ] 1.2 Extract PinkIK group target-frame and base-link validation changes. +- [ ] 1.3 Extract Jacobian IK group selection changes. +- [ ] 1.4 Extract Drake optimization IK group target-frame changes. +- [ ] 1.5 Extract RRT planner group-local target and projection changes. + +## 2. Tests + +- [ ] 2.1 Bring over PinkIK tests relevant to group target frames and base-link validation. +- [ ] 2.2 Bring over Jacobian IK selection tests. +- [ ] 2.3 Bring over RRT planner selection/group tests. + +## 3. Validation + +- [ ] 3.1 Run `uv run pytest dimos/manipulation/planning/kinematics/test_pink_ik.py dimos/manipulation/planning/kinematics/test_jacobian_ik_selection.py dimos/manipulation/planning/planners/test_rrt_planner_selection.py -q`. +- [ ] 3.2 Run targeted mypy on changed algorithm files. +- [ ] 3.3 Optional manual smoke: solve IK and plan a small reachable group pose; print status and path length. diff --git a/openspec/changes/manipulation-module-group-api/.openspec.yaml b/openspec/changes/manipulation-module-group-api/.openspec.yaml new file mode 100644 index 0000000000..34f9314d22 --- /dev/null +++ b/openspec/changes/manipulation-module-group-api/.openspec.yaml @@ -0,0 +1,2 @@ +schema: spec-driven +created: 2026-06-29 diff --git a/openspec/changes/manipulation-module-group-api/README.md b/openspec/changes/manipulation-module-group-api/README.md new file mode 100644 index 0000000000..a4cb1c316f --- /dev/null +++ b/openspec/changes/manipulation-module-group-api/README.md @@ -0,0 +1,3 @@ +# manipulation-module-group-api + +Extract PR 4 from reference branch cc/spec/movegroup: expose group-aware planning through ManipulationModule while preserving safe robot-scoped compatibility behavior. diff --git a/openspec/changes/manipulation-module-group-api/design.md b/openspec/changes/manipulation-module-group-api/design.md new file mode 100644 index 0000000000..5e84d72704 --- /dev/null +++ b/openspec/changes/manipulation-module-group-api/design.md @@ -0,0 +1,28 @@ +## Context + +The manipulation module is where planning groups become user-facing. It must expose explicit group APIs while retaining safe behavior for existing robot-scoped wrappers. + +## Goals / Non-Goals + +**Goals:** +- Support explicit group IDs for joint targets, pose targets, IK, previews, and robot info. +- Keep robot-scoped wrappers deterministic: no unique pose group means no successful robot-scoped pose action. +- Preserve safe return contracts for RPC-like public methods. +- Cover compatibility behavior in unit tests. + +**Non-Goals:** +- Do not implement Viser panel behavior in this PR. +- Do not add new planner algorithms. +- Do not change control task behavior unless required by module API compilation. + +## Decisions + +- `get_ee_pose` returns `None` when no unique pose-targetable group exists. +- `plan_to_pose` returns `False` when no unique pose-targetable group exists. +- `inverse_kinematics_single` returns `IKResult(NO_SOLUTION, message=...)` when no unique pose-targetable group exists. +- Explicit group APIs are preferred for multi-group robots. + +## Risks / Trade-offs + +- This file has a large diff. Keep the PR focused on API behavior and tests; do not include UI state-machine changes. +- Some compatibility wrappers are intentionally conservative to avoid silently selecting the wrong group. diff --git a/openspec/changes/manipulation-module-group-api/proposal.md b/openspec/changes/manipulation-module-group-api/proposal.md new file mode 100644 index 0000000000..a74f5a3adf --- /dev/null +++ b/openspec/changes/manipulation-module-group-api/proposal.md @@ -0,0 +1,25 @@ +## Why + +Once engines and algorithms are group-aware, the public manipulation module must expose those capabilities safely. This PR is the reviewer-facing API migration and should stay separate from the Viser UI rewrite. + +## What Changes + +- Add or migrate `ManipulationModule` APIs for group-aware joint targets, pose targets, previews, IK, and robot info. +- Preserve robot-scoped compatibility wrappers with explicit behavior when there is no unique pose-targetable group. +- Update coordinator client and example client call sites. +- Add module/unit/e2e tests for public behavior. + +## Capabilities + +### New Capabilities +- `manipulation-module-group-api`: Public manipulation APIs support planning-group IDs and safe compatibility wrappers. + +### Modified Capabilities +- `manipulation-module-planning`: Robot-scoped pose wrappers no longer assume an arbitrary or hidden end-effector. + +## Impact + +- Base branch: PR 3 `group-aware-ik-rrt`. +- Reference implementation: `cc/spec/movegroup`, including Greptile follow-up behavior for robot-scoped wrappers. +- Primary files: `dimos/manipulation/manipulation_module.py`, coordinator/example clients, manipulation module tests, and planning-group e2e test. +- Out of scope: Viser UI and control/task changes. diff --git a/openspec/changes/manipulation-module-group-api/specs/manipulation-module-group-api/spec.md b/openspec/changes/manipulation-module-group-api/specs/manipulation-module-group-api/spec.md new file mode 100644 index 0000000000..16aa9bb6bc --- /dev/null +++ b/openspec/changes/manipulation-module-group-api/specs/manipulation-module-group-api/spec.md @@ -0,0 +1,23 @@ +## ADDED Requirements + +### Requirement: Manipulation module must expose explicit group planning APIs +The manipulation module MUST allow callers to target planning groups explicitly for joint and pose planning operations. + +#### Scenario: Caller plans to a group pose +- **WHEN** a caller submits a pose target for a valid group ID +- **THEN** the module plans using that group and returns the planning result through the existing public contract + +### Requirement: Robot-scoped pose wrappers must fail safely without a unique pose group +Robot-scoped pose wrapper APIs MUST fail safely when a selected robot has zero or multiple pose-targetable groups. + +#### Scenario: End-effector pose is requested with no unique pose group +- **WHEN** `get_ee_pose` cannot resolve exactly one pose-targetable group +- **THEN** it returns `None` + +#### Scenario: Robot-scoped pose planning is requested with no unique pose group +- **WHEN** `plan_to_pose` cannot resolve exactly one pose-targetable group +- **THEN** it returns `False` + +#### Scenario: Robot-scoped IK is requested with no unique pose group +- **WHEN** `inverse_kinematics_single` cannot resolve exactly one pose-targetable group +- **THEN** it returns an `IKResult` with `NO_SOLUTION` diff --git a/openspec/changes/manipulation-module-group-api/tasks.md b/openspec/changes/manipulation-module-group-api/tasks.md new file mode 100644 index 0000000000..7f50e78293 --- /dev/null +++ b/openspec/changes/manipulation-module-group-api/tasks.md @@ -0,0 +1,18 @@ +## 1. Extract module API changes + +- [ ] 1.1 Start from PR 3 and use `cc/spec/movegroup` as reference. +- [ ] 1.2 Extract group-aware target, IK, preview, robot-info, and execution behavior from `ManipulationModule`. +- [ ] 1.3 Extract coordinator client and example client updates only as needed for the public API. +- [ ] 1.4 Include Greptile follow-up behavior for `get_ee_pose` and `plan_to_pose`. + +## 2. Tests + +- [ ] 2.1 Bring over focused `test_manipulation_unit.py` coverage for group APIs and compatibility wrappers. +- [ ] 2.2 Bring over relevant `test_manipulation_module.py` integration coverage. +- [ ] 2.3 Bring over `dimos/e2e_tests/test_manipulation_planning_groups.py` if it can run against this stacked base. + +## 3. Validation + +- [ ] 3.1 Run `uv run pytest dimos/manipulation/test_manipulation_unit.py dimos/manipulation/test_manipulation_module.py dimos/e2e_tests/test_manipulation_planning_groups.py -q`. +- [ ] 3.2 Run `uv run mypy dimos/manipulation/manipulation_module.py`. +- [ ] 3.3 Verify no Viser implementation files are included. diff --git a/openspec/changes/viser-group-planning-ui/.openspec.yaml b/openspec/changes/viser-group-planning-ui/.openspec.yaml new file mode 100644 index 0000000000..34f9314d22 --- /dev/null +++ b/openspec/changes/viser-group-planning-ui/.openspec.yaml @@ -0,0 +1,2 @@ +schema: spec-driven +created: 2026-06-29 diff --git a/openspec/changes/viser-group-planning-ui/README.md b/openspec/changes/viser-group-planning-ui/README.md new file mode 100644 index 0000000000..72628be56b --- /dev/null +++ b/openspec/changes/viser-group-planning-ui/README.md @@ -0,0 +1,3 @@ +# viser-group-planning-ui + +Extract PR 5 from reference branch cc/spec/movegroup: make Viser visualization and panel workflows group-aware. diff --git a/openspec/changes/viser-group-planning-ui/design.md b/openspec/changes/viser-group-planning-ui/design.md new file mode 100644 index 0000000000..0e201b7004 --- /dev/null +++ b/openspec/changes/viser-group-planning-ui/design.md @@ -0,0 +1,28 @@ +## Context + +Viser must let users select planning groups, move joint and pose targets, see feasibility feedback, preview paths, and execute only when the preview still matches the current robot state. + +## Goals / Non-Goals + +**Goals:** +- Expose group selection and group-aware robot controls. +- Keep target ghost and preview animation tied to selected groups. +- Preserve safe execution behavior and clear recoverable errors. +- Keep UI review isolated from planner correctness review. + +**Non-Goals:** +- Do not change core planning algorithms in this PR. +- Do not alter the planning-group data model. +- Do not include control/coordinator changes. + +## Decisions + +- Use a panel backend boundary so UI code does not directly own all manipulation-module orchestration. +- Treat planning-group IDs as the UI's stable selection keys. +- Validate base-link/root assumptions before rendering URDFs under base poses. +- Keep manual demo/checklist coverage for UI feel in addition to unit tests. + +## Risks / Trade-offs + +- UI tests can pass while interaction feels wrong; require a small manual checklist before review. +- This PR is still large, but isolating it keeps visual review focused. diff --git a/openspec/changes/viser-group-planning-ui/proposal.md b/openspec/changes/viser-group-planning-ui/proposal.md new file mode 100644 index 0000000000..54137b817c --- /dev/null +++ b/openspec/changes/viser-group-planning-ui/proposal.md @@ -0,0 +1,25 @@ +## Why + +The Viser changes are large and have different review criteria from planning algorithms. They should be reviewed as a dedicated UI/backend PR after the public manipulation group APIs exist. + +## What Changes + +- Make the Viser panel group-aware. +- Split panel/backend behavior out of the old adapter shape. +- Update scene previews, target ghosts, group selection, feasibility state, and safe execution checks. +- Update Viser tests and manual review checklist. + +## Capabilities + +### New Capabilities +- `viser-group-planning-ui`: Viser visualization supports group-aware planning, preview, target evaluation, and execution state. + +### Modified Capabilities +- `manipulation-visualization`: Visualization targets explicit planning groups rather than a robot-scoped end-effector field. + +## Impact + +- Base branch: PR 4 `manipulation-module-group-api`. +- Reference implementation: `cc/spec/movegroup`. +- Primary files: `dimos/manipulation/visualization/viser/*`, `dimos/manipulation/visualization/types.py`, `dimos/manipulation/visualization/test_factory.py`. +- Out of scope: planning model/backend/algorithm changes except as already supplied by earlier PRs. diff --git a/openspec/changes/viser-group-planning-ui/specs/viser-group-planning-ui/spec.md b/openspec/changes/viser-group-planning-ui/specs/viser-group-planning-ui/spec.md new file mode 100644 index 0000000000..a653991fcd --- /dev/null +++ b/openspec/changes/viser-group-planning-ui/specs/viser-group-planning-ui/spec.md @@ -0,0 +1,22 @@ +## ADDED Requirements + +### Requirement: Viser panel must select planning groups explicitly +The Viser planning panel MUST present and use planning-group selections for group-aware joint and pose planning controls. + +#### Scenario: User selects a manipulator group +- **WHEN** a user selects a planning group in the panel +- **THEN** joint controls, pose target controls, and preview state apply to that group + +### Requirement: Viser preview must reflect group feasibility and current state +The Viser UI MUST show target feasibility and preview validity for the selected planning group. + +#### Scenario: Target is infeasible +- **WHEN** target evaluation reports infeasible for the selected group +- **THEN** the target ghost and panel state indicate that the target cannot be executed safely + +### Requirement: Viser execution must require a fresh matching plan +The Viser panel MUST prevent execution when no fresh plan matches the current selected robot/group state. + +#### Scenario: Current state no longer matches preview +- **WHEN** the robot state changes after planning +- **THEN** the execute action is rejected until a fresh matching plan is generated diff --git a/openspec/changes/viser-group-planning-ui/tasks.md b/openspec/changes/viser-group-planning-ui/tasks.md new file mode 100644 index 0000000000..0e9523358e --- /dev/null +++ b/openspec/changes/viser-group-planning-ui/tasks.md @@ -0,0 +1,17 @@ +## 1. Extract Viser UI changes + +- [ ] 1.1 Start from PR 4 and use `cc/spec/movegroup` as reference. +- [ ] 1.2 Extract Viser panel/backend/scene/state/visualizer group-aware changes. +- [ ] 1.3 Remove or replace the old adapter code only as required by this UI slice. +- [ ] 1.4 Extract visualization type/factory updates needed by Viser. + +## 2. Tests and manual check + +- [ ] 2.1 Bring over Viser unit tests and lifecycle tests. +- [ ] 2.2 Run the manual checklist: group selector, joint sliders, pose gizmo, target ghost, infeasible target color, path preview, execute gate, clear path. + +## 3. Validation + +- [ ] 3.1 Run `uv run pytest dimos/manipulation/visualization/test_factory.py dimos/manipulation/visualization/viser/test_*.py -q`. +- [ ] 3.2 Run targeted mypy on changed Viser production files if practical. +- [ ] 3.3 Verify no planner/backend implementation changes are included beyond compile fixes against PR 4. diff --git a/openspec/config.yaml b/openspec/config.yaml new file mode 100644 index 0000000000..392946c67c --- /dev/null +++ b/openspec/config.yaml @@ -0,0 +1,20 @@ +schema: spec-driven + +# Project context (optional) +# This is shown to AI when creating artifacts. +# Add your tech stack, conventions, style guides, domain knowledge, etc. +# Example: +# context: | +# Tech stack: TypeScript, React, Node.js +# We use conventional commits +# Domain: e-commerce platform + +# Per-artifact rules (optional) +# Add custom rules for specific artifacts. +# Example: +# rules: +# proposal: +# - Keep proposals under 500 words +# - Always include a "Non-goals" section +# tasks: +# - Break tasks into chunks of max 2 hours diff --git a/openspec/specs/manipulation-planning-config/spec.md b/openspec/specs/manipulation-planning-config/spec.md new file mode 100644 index 0000000000..bd13e49196 --- /dev/null +++ b/openspec/specs/manipulation-planning-config/spec.md @@ -0,0 +1,10 @@ +# manipulation-planning-config Specification + +## Requirements + +### Requirement: Robot model configuration must separate placement from planning chains +Robot model configuration MUST treat robot-scoped `base_link` as the link placed by `base_pose` and used by backend weld/strip behavior. Planning-chain base links and pose target frames MUST come from planning groups. + +#### Scenario: Robot has one pose-targetable group +- **WHEN** a robot model has `base_link="base_link"` and a planning group with `base_link="link1"` and `tip_link="tcp"` +- **THEN** placement uses the robot-scoped `base_link` while pose planning targets `tcp` diff --git a/openspec/specs/planning-group-foundation/spec.md b/openspec/specs/planning-group-foundation/spec.md new file mode 100644 index 0000000000..6124a3bf5c --- /dev/null +++ b/openspec/specs/planning-group-foundation/spec.md @@ -0,0 +1,24 @@ +# planning-group-foundation Specification + +## Requirements + +### Requirement: Planning group definitions must identify kinematic chains +The planning configuration MUST represent kinematic chain metadata with planning-group definitions containing group-local joint names, a chain base link, and an optional pose-target tip link. + +#### Scenario: Explicit manipulator group is configured +- **WHEN** a manipulator config declares a group with local joints, `base_link`, and `tip_link` +- **THEN** the resolved robot model exposes that group without relying on a robot-scoped end-effector field + +### Requirement: Robot configuration must reject removed end-effector metadata +Robot configuration MUST NOT accept robot-scoped `end_effector_link` as the source of pose-target metadata. It MUST direct callers to planning-group `tip_link` values or SRDF discovery. + +#### Scenario: Legacy end-effector field is provided +- **WHEN** a robot config is constructed with `end_effector_link` +- **THEN** validation fails with migration guidance + +### Requirement: Group identifiers must be globally scoped +Planning-group APIs MUST use stable group identifiers scoped by robot name so multi-robot stacks can distinguish groups with the same local name. + +#### Scenario: Two robots use manipulator groups +- **WHEN** two robot configs each define a `manipulator` group +- **THEN** their resolved group IDs are distinct diff --git a/openspec/specs/planning-world-monitor-groups/spec.md b/openspec/specs/planning-world-monitor-groups/spec.md new file mode 100644 index 0000000000..5a737525bc --- /dev/null +++ b/openspec/specs/planning-world-monitor-groups/spec.md @@ -0,0 +1,26 @@ +## Purpose + +Specify world and monitor support for planning-group queries. + +## Requirements + +### Requirement: World backends must answer group pose queries +World backends MUST provide end-effector pose queries for pose-targetable planning groups using the group's configured target frame. + +#### Scenario: Group pose is requested +- **WHEN** a caller requests FK for a valid pose-targetable group ID +- **THEN** the backend returns the pose of that group's `tip_link` + +### Requirement: World backends must return group-ordered Jacobians +World backends MUST return Jacobian columns ordered according to the requested group's local joint names. + +#### Scenario: Backend returns a full robot Jacobian +- **WHEN** a group contains a subset of robot joints in a custom order +- **THEN** the backend projects and reorders Jacobian columns to match the group + +### Requirement: Monitors must expose group-scoped state +World monitors MUST support planning-group state queries without requiring callers to infer robot-local joint mappings. + +#### Scenario: Group state is requested for a multi-group robot +- **WHEN** a caller requests state for a specific group ID +- **THEN** the monitor returns joint data scoped and ordered for that group