diff --git a/dimos/manipulation/planning/groups/test_planning_groups.py b/dimos/manipulation/planning/groups/test_planning_groups.py
index ae5776fd74..8b399354a6 100644
--- a/dimos/manipulation/planning/groups/test_planning_groups.py
+++ b/dimos/manipulation/planning/groups/test_planning_groups.py
@@ -20,7 +20,6 @@
import pytest
-from dimos.manipulation.planning.groups import utils as planning_group_utils
from dimos.manipulation.planning.groups.discovery import (
FALLBACK_PLANNING_GROUP_NAME,
PlanningGroupDiscoveryError,
@@ -33,6 +32,7 @@
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,
@@ -474,21 +474,35 @@ def test_matching_global_joint_name_requires_unique_suffix_match() -> None:
assert matching_global_joint_name({"left/j1": 1.0}, "j2") is None
-def test_matching_global_joint_name_warns_when_suffix_match_is_not_unique(
- monkeypatch: pytest.MonkeyPatch,
-) -> None:
- messages: list[str] = []
+def test_filter_joint_state_to_selected_joints_uses_local_fallbacks() -> None:
+ state = JointState(name=["j2", "arm/j1"], position=[2.0, 1.0])
- def capture_warning(message: str) -> None:
- messages.append(message)
+ filtered = filter_joint_state_to_selected_joints(
+ state,
+ ["arm/j1", "arm/j2"],
+ ["j1", "j2"],
+ )
- monkeypatch.setattr(planning_group_utils.logger, "warning", capture_warning)
+ assert filtered.name == ["arm/j1", "arm/j2"]
+ assert filtered.position == [1.0, 2.0]
- assert matching_global_joint_name({"left/j1": 1.0, "right/j1": 2.0}, "j1") is None
- assert messages == [
- "Expected exactly one global joint ending with local joint name 'j1', "
- "found 2 matches: ['left/j1', 'right/j1']"
- ]
+
+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:
@@ -569,3 +583,76 @@ def test_robot_model_config_end_effector_link_rejects_ambiguous_pose_groups() ->
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
index d128d18e3a..283681adcc 100644
--- a/dimos/manipulation/planning/groups/utils.py
+++ b/dimos/manipulation/planning/groups/utils.py
@@ -16,6 +16,9 @@
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,
@@ -28,9 +31,6 @@
PlanningGroupID,
)
from dimos.msgs.sensor_msgs.JointState import JointState
-from dimos.utils.logging_config import setup_logger
-
-logger = setup_logger()
def planning_group_id_from_selector(selector: PlanningGroupID | PlanningGroup) -> PlanningGroupID:
@@ -48,10 +48,6 @@ def matching_global_joint_name(
matches = [name for name in positions_by_name if name.endswith(suffix)]
if len(matches) == 1:
return matches[0]
- logger.warning(
- f"Expected exactly one global joint ending with local joint name "
- f"'{local_joint_name}', found {len(matches)} matches: {matches}"
- )
return None
@@ -83,7 +79,7 @@ def filter_joint_state_to_selected_joints(
missing.append(global_name)
if missing:
- raise ValueError(f"Joint state is missing selected joints: {missing}")
+ raise ValueError(f"IK result is missing selected joints: {missing}")
return JointState({"name": list(global_joint_names), "position": selected_positions})
@@ -141,3 +137,44 @@ def joint_target_to_global_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/monitor/test_world_monitor.py b/dimos/manipulation/planning/monitor/test_world_monitor.py
index 355c1e6712..a78957858c 100644
--- a/dimos/manipulation/planning/monitor/test_world_monitor.py
+++ b/dimos/manipulation/planning/monitor/test_world_monitor.py
@@ -17,6 +17,9 @@
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
@@ -25,15 +28,48 @@
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 []
@@ -70,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):
@@ -96,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
@@ -165,6 +211,26 @@ def _robot_config() -> RobotModelConfig:
)
+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",
+ )
+ ],
+ }
+ )
+
+
def test_world_monitor_add_robot_records_scene_without_visualization_probe() -> None:
fake_world = FakeWorld()
fake_viz = FakeViz()
@@ -209,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/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_roboplan_world.py b/dimos/manipulation/test_roboplan_world.py
index 65e4000a03..32eaa97bfa 100644
--- a/dimos/manipulation/test_roboplan_world.py
+++ b/dimos/manipulation/test_roboplan_world.py
@@ -387,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:
@@ -473,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/openspec/changes/planning-groups-foundation/.openspec.yaml b/openspec/changes/planning-groups-foundation/.openspec.yaml
deleted file mode 100644
index 34f9314d22..0000000000
--- a/openspec/changes/planning-groups-foundation/.openspec.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-schema: spec-driven
-created: 2026-06-29
diff --git a/openspec/changes/planning-groups-foundation/README.md b/openspec/changes/planning-groups-foundation/README.md
deleted file mode 100644
index 820f444c27..0000000000
--- a/openspec/changes/planning-groups-foundation/README.md
+++ /dev/null
@@ -1,3 +0,0 @@
-# planning-groups-foundation
-
-Extract PR 1 from reference branch cc/spec/movegroup: introduce planning-group foundations, RobotConfig group generation, manipulator group declarations, and foundation docs/tests.
diff --git a/openspec/changes/planning-groups-foundation/design.md b/openspec/changes/planning-groups-foundation/design.md
deleted file mode 100644
index 287006f709..0000000000
--- a/openspec/changes/planning-groups-foundation/design.md
+++ /dev/null
@@ -1,29 +0,0 @@
-## Context
-
-The current branch introduces planning groups across the stack, but the reviewable foundation is the abstraction itself: stable group IDs, local/global joint mapping, group definitions, RobotConfig discovery/generation, and removal of robot-scoped end-effector metadata.
-
-## Goals / Non-Goals
-
-**Goals:**
-- Define planning groups as the only source for chain base links and pose target tip links.
-- Preserve robot-scoped `base_link` as placement/weld/strip metadata, not a planning-chain selector.
-- Let robot configs create explicit or discovered planning groups.
-- Provide small, direct tests that prove group IDs, joint-name mapping, discovery, and strict config validation.
-
-**Non-Goals:**
-- Do not migrate world backends or planners in this PR.
-- Do not migrate `ManipulationModule` public APIs in this PR.
-- Do not include visualization or control changes.
-
-## Decisions
-
-- Use `PlanningGroupDefinition` for config-time group declarations and `PlanningGroup` for resolved runtime group IDs.
-- Use globally scoped group IDs such as `/` at API boundaries.
-- Keep `RobotModelConfig.base_link` for robot placement semantics only.
-- Reject legacy robot-scoped `end_effector_link`; pose target frames come from group `tip_link` or SRDF discovery.
-- Keep this PR compatible enough for later stacked PRs; if a later change is needed for tests, prefer a small compatibility wrapper over pulling later files forward.
-
-## Risks / Trade-offs
-
-- Earlier PRs may need temporary compatibility paths because the full branch removes legacy behavior across several layers.
-- Discovery behavior must be clear: high-level `RobotConfig.to_robot_model_config()` can discover/fallback groups, while direct `RobotModelConfig(...)` requires explicit groups.
diff --git a/openspec/changes/planning-groups-foundation/proposal.md b/openspec/changes/planning-groups-foundation/proposal.md
deleted file mode 100644
index c6c7261bc1..0000000000
--- a/openspec/changes/planning-groups-foundation/proposal.md
+++ /dev/null
@@ -1,28 +0,0 @@
-## Why
-
-The planning-group refactor is currently bundled in one large branch. Reviewers need a small first PR that establishes the core planning-group abstraction and robot configuration plumbing without also reviewing planner algorithms, module APIs, visualization, or control integration.
-
-This change extracts PR 1 from the reference branch/worktree `cc/spec/movegroup`. Treat the existing branch as a working reference implementation; do not redesign the feature unless required to keep this slice independently valid.
-
-## What Changes
-
-- Add planning-group identifiers, definitions, runtime models, registry helpers, and joint-name conversion utilities.
-- Update planning spec/config models so robot-scoped `end_effector_link` is removed and pose target frames are represented by planning-group `tip_link` values.
-- Add planning-group conversion/discovery support through manipulation planning config models.
-- Update manipulator robot configs to declare or discover explicit planning groups.
-- Add foundation-level unit tests and minimal concept docs.
-
-## Capabilities
-
-### New Capabilities
-- `planning-group-foundation`: Core planning-group data model, discovery, robot config generation, and validation contracts.
-
-### Modified Capabilities
-- `manipulation-planning-config`: Robot model configuration uses planning groups as the source of chain and pose-target metadata.
-
-## Impact
-
-- Base branch: `main`.
-- Reference implementation: branch/worktree `cc/spec/movegroup`; stabilize to a commit before assigning agents.
-- Primary files: `dimos/manipulation/planning/groups/*`, `dimos/manipulation/planning/spec/*`, `dimos/robot/manipulators/*/config.py`, `dimos/manipulation/planning/groups/test_planning_groups.py`, foundation tests, and planning-group docs.
-- Out of scope: world backends, IK/RRT algorithms, `ManipulationModule`, Viser UI, and control changes.
diff --git a/openspec/changes/planning-groups-foundation/tasks.md b/openspec/changes/planning-groups-foundation/tasks.md
deleted file mode 100644
index 0b625f1e79..0000000000
--- a/openspec/changes/planning-groups-foundation/tasks.md
+++ /dev/null
@@ -1,18 +0,0 @@
-## 1. Extract foundation files
-
-- [x] 1.1 Start from `main` and use `cc/spec/movegroup` as the reference implementation.
-- [x] 1.2 Bring over `dimos/manipulation/planning/groups/*`.
-- [x] 1.3 Bring over required planning spec/config changes only.
-- [x] 1.4 Bring over manipulator config group declarations without `dimos/robot/config.py`.
-
-## 2. Tests and docs
-
-- [x] 2.1 Bring over `dimos/manipulation/planning/groups/test_planning_groups.py`.
-- [x] 2.2 Remove stale `dimos/robot/test_config.py` scope from this PR.
-- [x] 2.3 Bring over only foundation docs needed to explain planning groups and custom-arm config.
-
-## 3. Validation
-
-- [x] 3.1 Run targeted planning-group tests under `dimos/manipulation/planning/groups/test_planning_groups.py`.
-- [x] 3.2 Run targeted mypy on changed production files.
-- [x] 3.3 Confirm no files from world/IK/RRT/module/Viser/control were pulled in accidentally.
diff --git a/openspec/changes/planning-world-monitor-groups/.openspec.yaml b/openspec/changes/planning-world-monitor-groups/.openspec.yaml
deleted file mode 100644
index 34f9314d22..0000000000
--- a/openspec/changes/planning-world-monitor-groups/.openspec.yaml
+++ /dev/null
@@ -1,2 +0,0 @@
-schema: spec-driven
-created: 2026-06-29
diff --git a/openspec/changes/planning-world-monitor-groups/README.md b/openspec/changes/planning-world-monitor-groups/README.md
deleted file mode 100644
index c5b27282f4..0000000000
--- a/openspec/changes/planning-world-monitor-groups/README.md
+++ /dev/null
@@ -1,3 +0,0 @@
-# planning-world-monitor-groups
-
-Extract PR 2 from reference branch cc/spec/movegroup: teach world backends and world monitors to answer planning-group FK, Jacobian, state, and collision queries.
diff --git a/openspec/changes/planning-world-monitor-groups/design.md b/openspec/changes/planning-world-monitor-groups/design.md
deleted file mode 100644
index eb1e36a5e6..0000000000
--- a/openspec/changes/planning-world-monitor-groups/design.md
+++ /dev/null
@@ -1,27 +0,0 @@
-## Context
-
-Planning groups become useful when world backends can answer queries for a specific group. This layer should prove that configured groups map correctly onto backend model instances, joint order, FK target frames, and Jacobian columns.
-
-## Goals / Non-Goals
-
-**Goals:**
-- Implement group FK/Jacobian support in Drake and RoboPlan worlds.
-- Make WorldMonitor expose group-scoped state and query helpers.
-- Validate group-local joint ordering and global/local joint-state mapping.
-- Keep base pose/base link semantics safe by rejecting unsupported backend cases rather than silently producing wrong transforms.
-
-**Non-Goals:**
-- Do not migrate planner or IK algorithms yet.
-- Do not expose new public module APIs yet.
-- Do not include Viser UI changes.
-
-## Decisions
-
-- Robot-scoped FK/Jacobian compatibility wrappers may remain temporarily but must resolve through exactly one pose-targetable group or fail clearly.
-- RoboPlan Jacobians must be projected into group-local joint order; returning a full backend Jacobian is not sufficient.
-- Backends should reject unsupported base-pose/base-link combinations explicitly.
-
-## Risks / Trade-offs
-
-- Backend capabilities differ. Tests should pin the supported subset and error messages.
-- Some existing tests may need compatibility shims until PR 3 migrates algorithms and module APIs.
diff --git a/openspec/changes/planning-world-monitor-groups/proposal.md b/openspec/changes/planning-world-monitor-groups/proposal.md
deleted file mode 100644
index 2d8d1681d1..0000000000
--- a/openspec/changes/planning-world-monitor-groups/proposal.md
+++ /dev/null
@@ -1,26 +0,0 @@
-## Why
-
-Once planning groups exist, the backend query layer must answer group-scoped FK, Jacobian, joint-state, and collision questions. This is the second reviewable PR in the stack: it consumes the foundation without exposing the public `ManipulationModule` API or UI rewrite.
-
-## What Changes
-
-- Teach Drake and RoboPlan world backends to resolve planning groups.
-- Add group FK/Jacobian APIs and group-local joint ordering behavior.
-- Update world monitor and robot state monitor logic for group-aware state access.
-- Preserve legacy robot-scoped wrappers only where needed for compatibility, resolving through a unique pose-targetable group.
-- Add backend and monitor tests.
-
-## Capabilities
-
-### New Capabilities
-- `planning-world-monitor-groups`: World and monitor support for planning-group queries.
-
-### Modified Capabilities
-- `manipulation-world-backends`: Drake/RoboPlan backends understand group-local chains and target frames.
-
-## Impact
-
-- Base branch: PR 1 `planning-groups-foundation`.
-- Reference implementation: `cc/spec/movegroup`.
-- Primary files: `dimos/manipulation/planning/world/*`, `dimos/manipulation/planning/monitor/*`, RoboPlan world tests, Drake planning-group tests.
-- Out of scope: IK/RRT algorithm migration, `ManipulationModule`, Viser UI, and control integration.
diff --git a/openspec/changes/planning-world-monitor-groups/tasks.md b/openspec/changes/planning-world-monitor-groups/tasks.md
deleted file mode 100644
index acb647d94a..0000000000
--- a/openspec/changes/planning-world-monitor-groups/tasks.md
+++ /dev/null
@@ -1,18 +0,0 @@
-## 1. Extract backend group support
-
-- [ ] 1.1 Start from PR 1 and use `cc/spec/movegroup` as reference.
-- [ ] 1.2 Extract Drake world group FK/Jacobian and base-link handling changes.
-- [ ] 1.3 Extract RoboPlan world group FK/Jacobian, joint-name normalization, and URDF strip handling changes.
-- [ ] 1.4 Extract world monitor and robot state monitor group-aware query changes.
-
-## 2. Tests
-
-- [ ] 2.1 Bring over Drake group world tests.
-- [ ] 2.2 Bring over RoboPlan world tests, including planning split file if needed.
-- [ ] 2.3 Bring over WorldMonitor tests for group state and ambiguity behavior.
-
-## 3. Validation
-
-- [ ] 3.1 Run `uv run pytest dimos/manipulation/planning/world/test_drake_world_planning_groups.py dimos/manipulation/test_roboplan_world.py dimos/manipulation/test_roboplan_world_planning.py dimos/manipulation/planning/monitor/test_world_monitor.py -q`.
-- [ ] 3.2 Run targeted mypy on changed backend/monitor files.
-- [ ] 3.3 Optional manual smoke: load a robot config, print group list, FK pose, Jacobian shape, and collision status.
diff --git a/openspec/changes/planning-groups-foundation/specs/manipulation-planning-config/spec.md b/openspec/specs/manipulation-planning-config/spec.md
similarity index 90%
rename from openspec/changes/planning-groups-foundation/specs/manipulation-planning-config/spec.md
rename to openspec/specs/manipulation-planning-config/spec.md
index ecb76b89b5..bd13e49196 100644
--- a/openspec/changes/planning-groups-foundation/specs/manipulation-planning-config/spec.md
+++ b/openspec/specs/manipulation-planning-config/spec.md
@@ -1,4 +1,6 @@
-## MODIFIED Requirements
+# 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.
diff --git a/openspec/changes/planning-groups-foundation/specs/planning-group-foundation/spec.md b/openspec/specs/planning-group-foundation/spec.md
similarity index 95%
rename from openspec/changes/planning-groups-foundation/specs/planning-group-foundation/spec.md
rename to openspec/specs/planning-group-foundation/spec.md
index 2da6cd9e9e..6124a3bf5c 100644
--- a/openspec/changes/planning-groups-foundation/specs/planning-group-foundation/spec.md
+++ b/openspec/specs/planning-group-foundation/spec.md
@@ -1,4 +1,6 @@
-## ADDED Requirements
+# 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.
diff --git a/openspec/changes/planning-world-monitor-groups/specs/planning-world-monitor-groups/spec.md b/openspec/specs/planning-world-monitor-groups/spec.md
similarity index 92%
rename from openspec/changes/planning-world-monitor-groups/specs/planning-world-monitor-groups/spec.md
rename to openspec/specs/planning-world-monitor-groups/spec.md
index 3bff6f8bd0..5a737525bc 100644
--- a/openspec/changes/planning-world-monitor-groups/specs/planning-world-monitor-groups/spec.md
+++ b/openspec/specs/planning-world-monitor-groups/spec.md
@@ -1,4 +1,8 @@
-## ADDED Requirements
+## 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.