diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index fc7047ff829..d819d2b346b 100644 --- a/dimos/control/coordinator.py +++ b/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 diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index 94ddad4fcd0..4abf3cbc680 100644 --- a/dimos/control/test_control.py +++ b/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): diff --git a/dimos/control/test_joint_commands.py b/dimos/control/test_joint_commands.py index ce2ba26e8a0..f4bf6b20a80 100644 --- a/dimos/control/test_joint_commands.py +++ b/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 []) diff --git a/dimos/control/test_per_robot_joint_states.py b/dimos/control/test_per_robot_joint_states.py index b7ba62a9054..e5fec4680b4 100644 --- a/dimos/control/test_per_robot_joint_states.py +++ b/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", ] diff --git a/dimos/control/tick_loop.py b/dimos/control/tick_loop.py index 075ec587ad4..9ff61ce50a5 100644 --- a/dimos/control/tick_loop.py +++ b/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]],