feat(control): publish accepted hardware position commands

dimos/control/coordinator.py

@@ -200,6 +200,8 @@ class Go2Coordinator(ControlCoordinator):
     coordinator_joint_state: Out[JointState]
     # Intended position-command updates for recording.
     coordinator_joint_command: Out[JointState]
+    # Position targets whose hardware write succeeded.
+    applied_joint_position_command: Out[JointState]
 
     # Input: Streaming joint commands for real-time control
     joint_command: In[JointState]
@@ -420,7 +422,14 @@ def _connect_robots(self) -> None:
                 message, a robot cannot be driven (see ``ConnectionSource``),
                 or this coordinator also has hardware adapters.
         """
-        commands = _robot_ports(self.outputs, COMMAND_PORTS)
+        commands = _robot_ports(
+            {
+                name: port
+                for name, port in self.outputs.items()
+                if name != "applied_joint_position_command"
+            },
+            COMMAND_PORTS,
+        )
         readings = _robot_ports(self.inputs, READING_PORTS)
         robots = list(dict.fromkeys((*commands, *readings)))
         if not robots:
@@ -1082,6 +1091,7 @@ def start(self) -> None:
             joint_to_hardware=self._joint_to_hardware,
             publish_callback=publish_cb,
             publish_command_callback=self.coordinator_joint_command.publish,
+            publish_accepted_command_callback=self.applied_joint_position_command.publish,
             publish_robot_callback=publish_robot_cb,
             publish_tf_callback=self._frame_pose_port().publish
             if self.config.publish_frame_poses

dimos/control/test_control.py

@@ -1206,7 +1206,9 @@ def test_unready_whole_body_is_excluded_from_read_and_write(self, mocker):
 
         state, per_hardware = tick_loop._read_all_hardware()
         imu = tick_loop._read_all_imu()
-        tick_loop._write_all_hardware({"g1": ({"g1/joint1": 0.25}, ControlMode.SERVO_POSITION)})
+        tick_loop._write_all_hardware(
+            {"g1": ({"g1/joint1": 0.25}, ControlMode.SERVO_POSITION)}, timestamp=123.0
+        )
 
         assert state.joint_positions == {}
         assert per_hardware == {}
@@ -1237,7 +1239,9 @@ def test_ready_whole_body_reads_and_writes(self):
         )
 
         state, _per_hardware = tick_loop._read_all_hardware()
-        tick_loop._write_all_hardware({"g1": ({"g1/joint1": 0.25}, ControlMode.SERVO_POSITION)})
+        tick_loop._write_all_hardware(
+            {"g1": ({"g1/joint1": 0.25}, ControlMode.SERVO_POSITION)}, timestamp=123.0
+        )
 
         assert state.joint_positions == {"g1/joint1": 0.5}
         adapter.write_motor_commands.assert_called_once()
@@ -1370,12 +1374,97 @@ def test_write_all_hardware_rejected_command_logs_error(self, mocker):
             joint_to_hardware={"arm/joint1": "arm"},
         )
 
-        tick_loop._write_all_hardware({"arm": ({"arm/joint1": 0.25}, ControlMode.SERVO_POSITION)})
+        tick_loop._write_all_hardware(
+            {"arm": ({"arm/joint1": 0.25}, ControlMode.SERVO_POSITION)}, timestamp=123.0
+        )
 
         log_error.assert_called_once_with(
             "Hardware arm rejected SERVO_POSITION command from control task"
         )
 
+    @pytest.mark.parametrize(
+        "mode,ready,accepted,error,expected",
+        [
+            (ControlMode.POSITION, True, True, None, [0.25]),
+            (ControlMode.SERVO_POSITION, True, True, None, [0.25]),
+            (ControlMode.VELOCITY, True, True, None, []),
+            (ControlMode.TORQUE, True, True, None, []),
+            (ControlMode.POSITION, False, True, None, []),
+            (ControlMode.POSITION, True, False, None, []),
+            (ControlMode.POSITION, True, True, RuntimeError("write failed"), []),
+        ],
+    )
+    def test_command_feedback_reports_only_successful_position_writes(
+        self, mocker, mode, ready, accepted, error, expected
+    ):
+        hardware = mocker.Mock(spec=ConnectedHardware)
+        hardware.ready_for_control.return_value = ready
+        hardware.write_command.return_value = accepted
+        hardware.write_command.side_effect = error
+        received = []
+        lock = threading.Lock()
+
+        def receive(message):
+            assert not lock.locked()
+            received.append(message)
+
+        tick_loop = TickLoop(
+            tick_rate=100.0,
+            hardware={"arm": hardware},
+            hardware_lock=lock,
+            tasks={},
+            task_lock=threading.Lock(),
+            joint_to_hardware={},
+            publish_accepted_command_callback=receive,
+            frame_id="robot",
+        )
+        tick_loop._write_all_hardware({"arm": ({"arm/joint1": 0.25}, mode)}, timestamp=123.0)
+
+        assert [value for msg in received for value in msg.position] == expected
+        assert [msg.name for msg in received] == ([["arm/joint1"]] if expected else [])
+        assert [(msg.ts, msg.frame_id) for msg in received] == (
+            [(123.0, "robot")] if expected else []
+        )
+        assert hardware.write_command.call_count == int(ready)
+
+    def test_command_feedback_combines_successful_writes_and_keeps_sparse_updates(self, mocker):
+        hardware = {
+            name: mocker.Mock(spec=ConnectedHardware) for name in ("left", "right", "failed")
+        }
+        for hw in hardware.values():
+            hw.ready_for_control.return_value = True
+            hw.write_command.return_value = True
+        hardware["failed"].write_command.return_value = False
+        received = []
+        tick_loop = TickLoop(
+            tick_rate=100.0,
+            hardware=hardware,
+            hardware_lock=threading.Lock(),
+            tasks={},
+            task_lock=threading.Lock(),
+            joint_to_hardware={},
+            publish_accepted_command_callback=received.append,
+        )
+        tick_loop._write_all_hardware(
+            {
+                "left": ({"left/joint1": 0.25}, ControlMode.POSITION),
+                "failed": ({"failed/joint1": 0.75}, ControlMode.POSITION),
+                "right": ({"right/gripper": 0.5}, ControlMode.SERVO_POSITION),
+            },
+            timestamp=123.0,
+        )
+        tick_loop._write_all_hardware(
+            {
+                "right": ({"right/gripper": 0.6}, ControlMode.SERVO_POSITION),
+            },
+            timestamp=124.0,
+        )
+
+        assert [(msg.name, msg.position) for msg in received] == [
+            (["left/joint1", "right/gripper"], [0.25, 0.5]),
+            (["right/gripper"], [0.6]),
+        ]
+
 
 class TestIntegration:
     def test_full_trajectory_execution(self, mock_adapter, wait_until):

dimos/control/test_joint_commands.py

@@ -27,7 +27,7 @@
 from dimos.msgs.sensor_msgs.JointState import JointState
 
 
-def make_loop(callback):
+def make_loop(callback, accepted_callback=None):
     hardware = MagicMock(spec=ConnectedHardware)
     hardware.joint_names = ["arm/joint2", "arm/joint1", "arm/gripper"]
     hardware.component = HardwareComponent(
@@ -46,6 +46,7 @@ def make_loop(callback):
         task_lock=threading.Lock(),
         joint_to_hardware={},
         publish_command_callback=callback,
+        publish_accepted_command_callback=accepted_callback,
         frame_id="robot",
     ), hardware
 
@@ -137,3 +138,14 @@ def test_coordinator_wires_joint_state_command_output():
             )
         finally:
             coordinator.stop()
+
+
+@pytest.mark.parametrize("success", [True, False])
+def test_intended_and_accepted_targets_have_distinct_write_semantics(success):
+    intended = []
+    accepted = []
+    tick, hardware = make_loop(intended.append, accepted.append)
+    hardware.write_command.return_value = success
+    tick._write_all_hardware({"arm": ({"arm/joint1": 1.2}, ControlMode.POSITION)}, timestamp=123.0)
+    assert [message.position for message in intended] == [[1.2]]
+    assert [message.position for message in accepted] == ([[1.2]] if success else [])

dimos/control/test_per_robot_joint_states.py

@@ -170,6 +170,7 @@ def test_plain_coordinator_needs_no_per_robot_ports(self, make_coordinator):
 
         assert sorted(coordinator.list_hardware()) == ["left_arm", "right_arm"]
         assert sorted(coordinator.outputs) == [
+            "applied_joint_position_command",
             "coordinator_joint_command",
             "coordinator_joint_state",
         ]

dimos/control/tick_loop.py

@@ -116,6 +116,7 @@ def __init__(
         frame_id: str = "coordinator",
         log_ticks: bool = False,
         publish_command_callback: Callable[[JointState], None] | None = None,
+        publish_accepted_command_callback: Callable[[JointState], None] | None = None,
         connections: dict[str, ConnectionSource] | None = None,
         connection_command_callback: Callable[[str, dict[str, float]], None] | None = None,
     ) -> None:
@@ -134,6 +135,7 @@ def __init__(
         self._joint_to_hardware = joint_to_hardware
         self._publish_callback = publish_callback
         self._publish_command_callback = publish_command_callback
+        self._publish_accepted_command_callback = publish_accepted_command_callback
         self._publish_robot_callback = publish_robot_callback
         self._publish_tf_callback = publish_tf_callback
         self._frame_pose_period = 1.0 / frame_pose_hz if publish_tf_callback else 0.0
@@ -228,7 +230,7 @@ def _tick(self) -> None:
 
         hw_commands = self._route_to_hardware(joint_commands)
 
-        self._write_all_hardware(hw_commands)
+        self._write_all_hardware(hw_commands, joint_states.timestamp)
 
         self._write_all_connections(hw_commands)
 
@@ -470,10 +472,13 @@ def _route_to_hardware(
     def _write_all_hardware(
         self,
         hw_commands: dict[str, tuple[dict[str, float], ControlMode]],
+        timestamp: float | None = None,
     ) -> None:
-        """Write commands to all hardware interfaces."""
+        """Write commands and publish intended and accepted targets separately."""
         hardware = self._hardware
         joint_commands: dict[str, float] = {}
+        accepted_names: list[str] = []
+        accepted_values: list[float] = []
         with self._hardware_lock:
             for hw_id, (positions, mode) in hw_commands.items():
                 if hw_id in hardware:
@@ -495,6 +500,9 @@ def _write_all_hardware(
                             logger.error(
                                 f"Hardware {hw_id} rejected {mode.name} command from control task"
                             )
+                        elif mode in (ControlMode.POSITION, ControlMode.SERVO_POSITION):
+                            accepted_names.extend(positions)
+                            accepted_values.extend(positions.values())
                     except Exception as e:
                         logger.error(f"Failed to write to {hw_id}: {e}")
         if joint_commands and self._publish_command_callback is not None:
@@ -512,6 +520,21 @@ def _write_all_hardware(
             except Exception:
                 logger.exception("Failed to publish joint commands")
 
+        if accepted_names and self._publish_accepted_command_callback is not None:
+            try:
+                self._publish_accepted_command_callback(
+                    JointState(
+                        ts=timestamp if timestamp is not None else time.time(),
+                        frame_id=self._frame_id,
+                        name=accepted_names,
+                        position=accepted_values,
+                        velocity=[],
+                        effort=[],
+                    )
+                )
+            except Exception:
+                logger.exception("Failed to publish accepted joint commands")
+
     def _write_all_connections(
         self,
         hw_commands: dict[str, tuple[dict[str, float], ControlMode]],