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.
 

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/test_dual.py

@@ -0,0 +1,74 @@
+# 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.coordinator import ControlCoordinator
+from dimos.core.coordination.blueprints import Blueprint
+from dimos.manipulation.manipulation_module import ManipulationModule
+from dimos.robot.manipulators.xarm.blueprints.basic import coordinator_dual_xarm
+
+
+def _dual_xarm6_planner() -> Blueprint:
+    from dimos.robot.manipulators.xarm.blueprints.basic import dual_xarm6_planner
+
+    return dual_xarm6_planner
+
+
+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_planner_blueprint_has_planner() -> None:
+    dual_xarm6_planner = _dual_xarm6_planner()
+    modules = [atom.module for atom in dual_xarm6_planner.blueprints]
+
+    assert ManipulationModule in modules
+
+
+def test_dual_xarm6_planner_uses_meshcat_visualization() -> None:
+    dual_xarm6_planner = _dual_xarm6_planner()
+    visualization = _manipulation_visualization(dual_xarm6_planner)
+
+    assert visualization == {"backend": "meshcat"}
+
+
+def test_dual_xarm6_planner_robots_use_global_joint_mapping() -> None:
+    dual_xarm6_planner = _dual_xarm6_planner()
+
+    for robot in _manipulation_robots(dual_xarm6_planner):
+        assert robot.coordinator_task_name == f"traj_{robot.name}"
+
+
+def test_dual_coordinator_xarm_task_names_match_manipulation_robot_defaults() -> None:
+    assert _coordinator_task_names(coordinator_dual_xarm) == [
+        "traj_left",
+        "traj_right",
+    ]

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}")