WIP: feat: add manipulation planning groups

AGENTS.md

@@ -44,7 +44,7 @@ dimos restart          # stop + re-run with same original args
 | `xarm-perception-sim-agent` | xArm | sim | GPT-4o | — | Manipulation + perception + agent, sim |
 | `xarm7-planner-coordinator` | xArm7 | real | — | — | Trajectory planner coordinator |
 | `teleop-quest-xarm7` | xArm7 | real | — | — | Quest VR teleop |
-| `dual-xarm6-planner` | xArm6×2 | real | — | — | Dual-arm motion planner |
+| `dual-xarm6-planner-coordinator` | xArm6×2 | real | — | — | Dual-arm motion planner + coordinator |
 
 Run `dimos list` for the full list.
 

CONTEXT.md

@@ -0,0 +1,89 @@
+# DimOS Robotics Language
+
+Shared vocabulary for DimOS robotics concepts. These terms define domain language, not implementation details.
+
+## Language
+
+**Robot Name**:
+A stable planning-domain identity for a concrete robot/model instance, used in public planning group and flat joint-name scoping.
+_Avoid_: World robot ID, hardware ID, namespace
+
+**World Robot ID**:
+An internal planning-world handle for a robot after it has been added to a backend world.
+_Avoid_: Robot name, hardware ID, joint namespace
+
+**Hardware ID**:
+A control-layer routing identity for a hardware component. For manipulator robots, it normally matches the robot name at the coordinator boundary.
+_Avoid_: Robot name when discussing planning semantics, world robot ID
+
+**Planning Group**:
+A named selectable serial kinematic chain of robot joints used as the unit of manipulation planning. After binding to a robot name, it is identified by a planning group ID and remains independent of backend world robot IDs.
+_Avoid_: Move group, movegroup
+
+**Planning Group Definition**:
+The model-level declaration of a planning group before it is bound to a concrete robot in a planning world.
+_Avoid_: Runtime group, robot ID
+
+**End-Effector Association**:
+Separate metadata used for pose-targeted operations. For a planning group defined by a chain, the end-effector link is the chain tip. For a planning group defined only by joints, there is no end-effector link.
+_Avoid_: Planning group definition
+
+**Planning Group Selection**:
+The set of one or more planning groups chosen for a planning request.
+_Avoid_: Composite group
+
+**Planning Target Set**:
+The atomic manipulation UI/planning state built on top of a planning group selection: selected planning groups, target authoring state, combined IK joint target, whole-set feasibility, and generated plan. Per-group UI panels are views into this target set, not independent planning states.
+_Avoid_: Independent group cards, per-robot plan state
+
+**Auxiliary Planning Group**:
+A planning group selected to participate in a planning target set without receiving a direct end-effector pose target in that request. It is solved, checked, planned, previewed, and executed with the whole target set; it simply has no assigned target gizmo. A planning group may be auxiliary in one request and directly targeted in another.
+_Avoid_: Joint-only group, intrinsic auxiliary group
+
+**Coordinated Planning Problem**:
+A planning request over one or more selected planning groups that is solved as one combined joint-space problem with one synchronized result.
+_Avoid_: Batch planning, independent planning
+
+**Planning Group ID**:
+An API-level identifier for a planning group, always written as `{robot_name}/{group_name}`. `/` is reserved as the delimiter and is not part of either component.
+_Avoid_: Bare group name, robot ID
+
+**Default Planning Group**:
+The generated fallback planning group used by robot-scoped compatibility APIs when no planning group is specified explicitly. It is not inferred from arbitrary SRDF group uniqueness.
+_Avoid_: Unique planning group, primary planning group
+
+**Robot-Scoped Compatibility API**:
+A convenience API that accepts a robot name for common single-robot calls but immediately delegates to planning-group APIs through the robot's default planning group. It does not define a separate planning model, storage model, or groupless execution path.
+_Avoid_: Robot-scoped planner, groupless API
+
+**Joint State**:
+A joint-name-keyed robot state that can represent any set of joints and is not inherently coupled to a robot, planning group, planning group selection, or joint-name scope. At flat multi-robot or coordinator boundaries, joint names are required and are global joint names. Robot identity and local-vs-global meaning are provided by the API boundary or containing type, not by extra fields on the generic joint state.
+_Avoid_: Planning-group-scoped state
+
+**Robot Model Joint Names**:
+The ordered controllable joints of a robot model in the model's local namespace. This usually aligns with the model's actuated joints, but is not itself a planning group.
+_Avoid_: Implicit planning group
+
+**Local Model Joint Name**:
+A joint name as it appears inside a robot model or SRDF before the model is bound to a concrete robot in a planning world.
+_Avoid_: Runtime joint name, coordinator joint name
+
+**Robot-Scoped Joint State**:
+A single-robot joint state whose robot identity is explicit outside the generic joint state. Robot-scoped APIs may accept unnamed ordered joint vectors in robot model joint order; when joint names are present, they are local model joint names because the robot identity is already explicit.
+_Avoid_: Namespaced local joint state, prefixed joint state
+
+**Generated Plan**:
+A flat planning result that may contain one or more robots. Joint states in a generated plan require names and use global joint names so the plan remains unambiguous across robot boundaries.
+_Avoid_: Robot-scoped joint plan, local joint plan
+
+**Group-Scoped Preview**:
+A visualization request for a generated plan over a planning group or planning group selection. A visualization backend may render the whole robot when partial-group rendering is not practical, but the API scope remains the generated plan's selected planning groups.
+_Avoid_: Robot-scoped preview API
+
+**Global Joint Name**:
+A boundary-level joint name that mechanically combines a robot name and local model joint name as `{robot_name}/{local_joint_name}` so it is stable and unique in flat joint-state representations, even when the local model joint name is already descriptive. `/` is reserved as the delimiter and is not part of either component.
+_Avoid_: Resolved joint name, coordinator joint name, bare joint name, local joint name
+
+**Robot Placement**:
+The placement of a robot model within the planning world. Robot placement belongs in the robot model description rather than in a separate planning configuration transform.
+_Avoid_: Planning base pose, config placement transform

dimos/control/README.md

@@ -27,7 +27,6 @@ Centralized control system for multi-arm robots with per-joint arbitration.
 ```bash
 # Terminal 1: Run coordinator
 dimos run coordinator-mock          # Single 7-DOF mock arm
-dimos run coordinator-dual-mock     # Dual arms (7+6 DOF)
 dimos run coordinator-piper-xarm    # Real hardware
 
 # Terminal 2: Control via CLI

dimos/control/blueprints/dual.py

@@ -15,31 +15,16 @@
 """Dual-arm coordinator blueprints with trajectory control.
 
 Usage:
-    dimos run coordinator-dual-mock      # Mock 7+6 DOF arms
     dimos run coordinator-dual-xarm      # XArm7 left + XArm6 right
     dimos run coordinator-piper-xarm     # XArm6 + Piper
 """
 
 from __future__ import annotations
 
-from dimos.control.blueprints._hardware import manipulator, mock_arm
+from dimos.control.blueprints._hardware import manipulator
 from dimos.control.coordinator import ControlCoordinator, TaskConfig
 from dimos.core.global_config import global_config
 
-# Dual mock arms (7-DOF left, 6-DOF right)
-_mock_left = mock_arm("left_arm", 7)
-_mock_right = mock_arm("right_arm", 6)
-
-coordinator_dual_mock = ControlCoordinator.blueprint(
-    hardware=[_mock_left, _mock_right],
-    tasks=[
-        TaskConfig(name="traj_left", type="trajectory", joint_names=_mock_left.joints, priority=10),
-        TaskConfig(
-            name="traj_right", type="trajectory", joint_names=_mock_right.joints, priority=10
-        ),
-    ],
-)
-
 # Dual XArm (XArm7 left, XArm6 right)
 _xarm7_left = manipulator(
     "left_arm",
@@ -58,10 +43,16 @@
     hardware=[_xarm7_left, _xarm6_right],
     tasks=[
         TaskConfig(
-            name="traj_left", type="trajectory", joint_names=_xarm7_left.joints, priority=10
+            name="traj_left_arm",
+            type="trajectory",
+            joint_names=_xarm7_left.joints,
+            priority=10,
         ),
         TaskConfig(
-            name="traj_right", type="trajectory", joint_names=_xarm6_right.joints, priority=10
+            name="traj_right_arm",
+            type="trajectory",
+            joint_names=_xarm6_right.joints,
+            priority=10,
         ),
     ],
 )
@@ -92,3 +83,9 @@
         ),
     ],
 )
+
+
+__all__ = [
+    "coordinator_dual_xarm",
+    "coordinator_piper_xarm",
+]

dimos/control/blueprints/test_dual.py

@@ -0,0 +1,69 @@
+# Copyright 2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Tests for dual-arm control blueprints."""
+
+from dimos.control.blueprints.dual import coordinator_dual_xarm
+from dimos.control.coordinator import ControlCoordinator
+from dimos.manipulation.blueprints import dual_xarm6_planner_coordinator
+from dimos.manipulation.manipulation_module import ManipulationModule
+from dimos.manipulation.planning.planning_identifiers import make_global_joint_names
+
+
+def _coordinator_task_names(blueprint) -> list[str]:
+    atom = next(atom for atom in blueprint.blueprints if atom.module is ControlCoordinator)
+    return [task.name for task in atom.kwargs["tasks"]]
+
+
+def _coordinator_tasks(blueprint):
+    atom = next(atom for atom in blueprint.blueprints if atom.module is ControlCoordinator)
+    return atom.kwargs["tasks"]
+
+
+def _manipulation_robots(blueprint):
+    atom = next(atom for atom in blueprint.blueprints if atom.module is ManipulationModule)
+    return atom.kwargs["robots"]
+
+
+def _manipulation_visualization(blueprint):
+    atom = next(atom for atom in blueprint.blueprints if atom.module is ManipulationModule)
+    return atom.kwargs["visualization"]
+
+
+def test_dual_xarm6_integrated_blueprint_has_planner_and_coordinator() -> None:
+    modules = [atom.module for atom in dual_xarm6_planner_coordinator.blueprints]
+
+    assert ManipulationModule in modules
+    assert ControlCoordinator in modules
+
+
+def test_dual_xarm6_integrated_blueprint_uses_viser_for_execution_ui() -> None:
+    visualization = _manipulation_visualization(dual_xarm6_planner_coordinator)
+
+    assert visualization == {"backend": "viser", "allow_plan_execute": True}
+
+
+def test_dual_xarm6_integrated_tasks_match_planner_robots() -> None:
+    tasks_by_name = {task.name: task for task in _coordinator_tasks(dual_xarm6_planner_coordinator)}
+
+    for robot in _manipulation_robots(dual_xarm6_planner_coordinator):
+        task = tasks_by_name[robot.coordinator_task_name]
+        assert task.joint_names == make_global_joint_names(robot.name, robot.joint_names)
+
+
+def test_dual_coordinator_xarm_task_names_match_manipulation_robot_defaults() -> None:
+    assert _coordinator_task_names(coordinator_dual_xarm) == [
+        "traj_left_arm",
+        "traj_right_arm",
+    ]

dimos/control/tasks/trajectory_task/trajectory_task.py

@@ -191,6 +191,23 @@ def execute(self, trajectory: JointTrajectory) -> bool:
             logger.warning(f"Empty trajectory for {self._name}")
             return False
 
+        if trajectory.joint_names and trajectory.joint_names != self._joint_names_list:
+            logger.warning(
+                f"Joint name mismatch for {self._name}: "
+                f"expected={self._joint_names_list}, received={trajectory.joint_names}"
+            )
+            return False
+
+        if not trajectory.joint_names:
+            expected_joint_count = len(self._joint_names_list)
+            for point in trajectory.points:
+                if len(point.positions) != expected_joint_count:
+                    logger.warning(
+                        f"Trajectory point dimension mismatch for {self._name}: "
+                        f"expected={expected_joint_count}, received={len(point.positions)}"
+                    )
+                    return False
+
         # Preempt any active trajectory
         if self._state == TrajectoryState.EXECUTING:
             logger.info(f"Preempting active trajectory on {self._name}")

dimos/control/test_control.py

@@ -274,6 +274,29 @@ def test_execute_trajectory(self, trajectory_task, simple_trajectory):
         assert trajectory_task.is_active()
         assert trajectory_task.get_state() == TrajectoryState.EXECUTING
 
+    def test_execute_rejects_mismatched_joint_names(self, trajectory_task):
+        trajectory = JointTrajectory(
+            joint_names=["other/joint1", "other/joint2", "other/joint3"],
+            points=[
+                TrajectoryPoint(
+                    positions=[0.0, 0.0, 0.0],
+                    velocities=[0.0, 0.0, 0.0],
+                    time_from_start=0.0,
+                ),
+                TrajectoryPoint(
+                    positions=[1.0, 0.5, 0.25],
+                    velocities=[0.0, 0.0, 0.0],
+                    time_from_start=1.0,
+                ),
+            ],
+        )
+
+        result = trajectory_task.execute(trajectory)
+
+        assert result is False
+        assert not trajectory_task.is_active()
+        assert trajectory_task.get_state() == TrajectoryState.IDLE
+
     def test_compute_during_trajectory(self, trajectory_task, simple_trajectory, coordinator_state):
         t_start = time.perf_counter()
         trajectory_task.execute(simple_trajectory)
@@ -530,6 +553,28 @@ def test_tick_loop_calls_compute(self, mock_adapter):
 
         assert mock_task.compute.call_count > 0
 
+    def test_write_all_hardware_logs_rejected_command(self, mocker):
+        hardware = {"arm": MagicMock()}
+        hardware["arm"].write_command.return_value = False
+        log_error = mocker.patch("dimos.control.tick_loop.logger.error")
+        tick_loop = TickLoop(
+            tick_rate=100.0,
+            hardware=hardware,
+            hardware_lock=threading.Lock(),
+            tasks={},
+            task_lock=threading.Lock(),
+            joint_to_hardware={},
+        )
+
+        tick_loop._write_all_hardware({"arm": ({"arm/joint1": 0.5}, ControlMode.SERVO_POSITION)})
+
+        hardware["arm"].write_command.assert_called_once_with(
+            {"arm/joint1": 0.5}, ControlMode.SERVO_POSITION
+        )
+        log_error.assert_called_once_with(
+            "Hardware %s rejected %d %s command(s)", "arm", 1, "SERVO_POSITION"
+        )
+
 
 class TestIntegration:
     def test_full_trajectory_execution(self, mock_adapter):

dimos/control/tick_loop.py

@@ -397,7 +397,13 @@ def _write_all_hardware(
             for hw_id, (positions, mode) in hw_commands.items():
                 if hw_id in self._hardware:
                     try:
-                        self._hardware[hw_id].write_command(positions, mode)
+                        if not self._hardware[hw_id].write_command(positions, mode):
+                            logger.error(
+                                "Hardware %s rejected %d %s command(s)",
+                                hw_id,
+                                len(positions),
+                                mode.name,
+                            )
                     except Exception as e:
                         logger.error(f"Failed to write to {hw_id}: {e}")
 

dimos/e2e_tests/test_control_coordinator.py

@@ -201,8 +201,8 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None:
         """Test dual-arm coordinator with independent trajectories."""
         lcm_spy.save_topic("/coordinator_joint_state#sensor_msgs.JointState")
 
-        # Start dual-arm mock coordinator
-        start_blueprint("coordinator-dual-mock")
+        # Start integrated dual-arm mock planner/coordinator
+        start_blueprint("dual-xarm6-planner-coordinator")
         lcm_spy.wait_for_saved_topic("/coordinator_joint_state#sensor_msgs.JointState")
 
         client = RPCClient(None, ControlCoordinator)
@@ -213,15 +213,15 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None:
             assert "right_arm/joint1" in joints
 
             tasks = client.list_tasks()
-            assert "traj_left" in tasks
-            assert "traj_right" in tasks
+            assert "traj_left_arm" in tasks
+            assert "traj_right_arm" in tasks
 
             # Create trajectories for both arms
             left_trajectory = JointTrajectory(
-                joint_names=[f"left_arm/joint{i + 1}" for i in range(7)],
+                joint_names=[f"left_arm/joint{i + 1}" for i in range(6)],
                 points=[
-                    TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 7),
-                    TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 7),
+                    TrajectoryPoint(time_from_start=0.0, positions=[0.0] * 6),
+                    TrajectoryPoint(time_from_start=0.5, positions=[0.2] * 6),
                 ],
             )
 
@@ -235,19 +235,20 @@ def test_dual_arm_coordinator(self, lcm_spy, start_blueprint) -> None:
 
             # Execute both via task_invoke
             assert (
-                client.task_invoke("traj_left", "execute", {"trajectory": left_trajectory}) is True
+                client.task_invoke("traj_left_arm", "execute", {"trajectory": left_trajectory})
+                is True
             )
             assert (
-                client.task_invoke("traj_right", "execute", {"trajectory": right_trajectory})
+                client.task_invoke("traj_right_arm", "execute", {"trajectory": right_trajectory})
                 is True
             )
 
             # Wait for completion
             time.sleep(1.0)
 
             # Both should complete
-            left_state = client.task_invoke("traj_left", "get_state")
-            right_state = client.task_invoke("traj_right", "get_state")
+            left_state = client.task_invoke("traj_left_arm", "get_state")
+            right_state = client.task_invoke("traj_right_arm", "get_state")
 
             assert left_state == TrajectoryState.COMPLETED
             assert right_state == TrajectoryState.COMPLETED