From 321724f0b8f6e22f16fb0ad20c546cf6a56b7e47 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 3 Jun 2026 14:35:17 -0700 Subject: [PATCH 1/6] fix: align keyboard teleop with live arm state --- dimos/robot/manipulators/xarm/blueprints.py | 12 +++++- .../teleop/keyboard/keyboard_teleop_module.py | 43 +++++++++++++++---- 2 files changed, 44 insertions(+), 11 deletions(-) diff --git a/dimos/robot/manipulators/xarm/blueprints.py b/dimos/robot/manipulators/xarm/blueprints.py index be8cf22b8c..6091f83030 100644 --- a/dimos/robot/manipulators/xarm/blueprints.py +++ b/dimos/robot/manipulators/xarm/blueprints.py @@ -53,7 +53,11 @@ # XArm6 mock sim + keyboard teleop + Drake visualization keyboard_teleop_xarm6 = autoconnect( - KeyboardTeleopModule.blueprint(model_path=XARM6_FK_MODEL, ee_joint_id=_xarm6_cfg.dof), + KeyboardTeleopModule.blueprint( + model_path=XARM6_FK_MODEL, + ee_joint_id=_xarm6_cfg.dof, + joint_names=_xarm6_cfg.coordinator_joint_names, + ), ControlCoordinator.blueprint( tick_rate=100.0, publish_joint_state=True, @@ -83,7 +87,11 @@ # XArm7 mock sim + keyboard teleop + Drake visualization keyboard_teleop_xarm7 = autoconnect( - KeyboardTeleopModule.blueprint(model_path=XARM7_FK_MODEL, ee_joint_id=_xarm7_cfg.dof), + KeyboardTeleopModule.blueprint( + model_path=XARM7_FK_MODEL, + ee_joint_id=_xarm7_cfg.dof, + joint_names=_xarm7_cfg.coordinator_joint_names, + ), ControlCoordinator.blueprint( tick_rate=100.0, publish_joint_state=True, diff --git a/dimos/teleop/keyboard/keyboard_teleop_module.py b/dimos/teleop/keyboard/keyboard_teleop_module.py index bc060c93d8..6cb85d3dba 100644 --- a/dimos/teleop/keyboard/keyboard_teleop_module.py +++ b/dimos/teleop/keyboard/keyboard_teleop_module.py @@ -19,7 +19,7 @@ Keyboard controls: W/S: +X/-X (forward/backward) - A/D: -Y/+Y (left/right) + A/D: +Y/-Y (left/right) Q/E: +Z/-Z (up/down) R/F: +Roll/-Roll T/G: +Pitch/-Pitch @@ -45,8 +45,9 @@ from dimos.control.examples.cartesian_ik_jogger import JogState from dimos.core.core import rpc from dimos.core.module import Module, ModuleConfig -from dimos.core.stream import Out +from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.JointState import JointState # Force X11 driver to avoid OpenGL threading issues os.environ["SDL_VIDEODRIVER"] = "x11" @@ -71,6 +72,8 @@ class KeyboardTeleopConfig(ModuleConfig): ee_joint_id: int = 6 task_name: str = "cartesian_ik_arm" home_joints: list[float] | None = None + joint_names: list[str] | None = None + initial_state_timeout: float = 5.0 class KeyboardTeleopModule(Module): @@ -81,6 +84,7 @@ class KeyboardTeleopModule(Module): config: KeyboardTeleopConfig + joint_state: In[JointState] cartesian_command: Out[PoseStamped] _stop_event: threading.Event @@ -107,14 +111,34 @@ def stop(self) -> None: self._thread.join(DEFAULT_THREAD_JOIN_TIMEOUT) super().stop() + def _read_joint_positions(self, timeout: float) -> list[float] | None: + try: + msg = self.joint_state.get_next(timeout) + except Exception: + return None + if not msg.position: + return None + + if joint_names := self.config.joint_names: + name_to_position = dict(zip(msg.name, msg.position, strict=False)) + if any(name not in name_to_position for name in joint_names): + return None + return [float(name_to_position[name]) for name in joint_names] + + if len(msg.position) < self.config.ee_joint_id: + return None + return [float(position) for position in msg.position[: self.config.ee_joint_id]] + def _pygame_loop(self) -> None: model_path = str(self.config.model_path) ee_joint_id = self.config.ee_joint_id task_name = self.config.task_name - # Initialize pose from forward kinematics at the robot's home configuration + initial_joints = self._read_joint_positions(self.config.initial_state_timeout) + if initial_joints is None: + return home_pose = JogState.from_fk(model_path, ee_joint_id, self.config.home_joints) - current_pose = home_pose.copy() + current_pose = JogState.from_fk(model_path, ee_joint_id, initial_joints).copy() # Publish initial pose self.cartesian_command.publish(current_pose.to_pose_stamped(task_name)) @@ -137,7 +161,8 @@ def _pygame_loop(self) -> None: if event.key == pygame.K_ESCAPE: self._stop_event.set() elif event.key == pygame.K_SPACE: - current_pose = home_pose.copy() + if joints := self._read_joint_positions(timeout=0.1): + current_pose = JogState.from_fk(model_path, ee_joint_id, joints).copy() keys = pygame.key.get_pressed() @@ -147,9 +172,9 @@ def _pygame_loop(self) -> None: if keys[pygame.K_s]: current_pose.x -= LINEAR_SPEED * dt if keys[pygame.K_a]: - current_pose.y -= LINEAR_SPEED * dt - if keys[pygame.K_d]: current_pose.y += LINEAR_SPEED * dt + if keys[pygame.K_d]: + current_pose.y -= LINEAR_SPEED * dt if keys[pygame.K_q]: current_pose.z += LINEAR_SPEED * dt if keys[pygame.K_e]: @@ -201,12 +226,12 @@ def _pygame_loop(self) -> None: controls = [ ("W/S", "+X/-X (forward/back)"), - ("A/D", "-Y/+Y (left/right)"), + ("A/D", "+Y/-Y (left/right)"), ("Q/E", "+Z/-Z (up/down)"), ("R/F", "+Roll/-Roll"), ("T/G", "+Pitch/-Pitch"), ("Y/H", "+Yaw/-Yaw"), - ("SPACE", "Reset to home"), + ("SPACE", "Sync to current pose"), ("ESC", "Quit"), ] for key, desc in controls: From 245583c8522e6782b173e077f81cdad420da7ad1 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 3 Jun 2026 16:35:35 -0700 Subject: [PATCH 2/6] feat: add xarm adapter lifecycle --- dimos/control/coordinator.py | 19 ++++++- dimos/control/test_control.py | 42 ++++++++++++++ dimos/hardware/manipulators/spec.py | 8 +++ dimos/hardware/manipulators/xarm/adapter.py | 61 +++++++++++++++++++++ 4 files changed, 128 insertions(+), 2 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 94d4837062..6512d77b8d 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -224,8 +224,13 @@ def _setup_hardware(self, component: HardwareComponent) -> None: raise RuntimeError(f"Failed to connect to {component.adapter_type} adapter") try: - if component.auto_enable and hasattr(adapter, "write_enable"): - adapter.write_enable(True) + if component.auto_enable: + activate = getattr(adapter, "activate", None) + if callable(activate): + if activate() is False: + raise RuntimeError(f"Failed to activate hardware {component.hardware_id}") + elif hasattr(adapter, "write_enable"): + adapter.write_enable(True) self.add_hardware(adapter, component) except Exception: @@ -706,6 +711,16 @@ def stop(self) -> None: if self._tick_loop: self._tick_loop.stop() + with self._hardware_lock: + for hw_id, interface in self._hardware.items(): + deactivate = getattr(interface.adapter, "deactivate", None) + if callable(deactivate): + try: + if deactivate() is False: + logger.error(f"Hardware {hw_id} deactivate returned False") + except Exception as e: + logger.error(f"Error deactivating hardware {hw_id}: {e}") + # Disconnect all hardware adapters with self._hardware_lock: for hw_id, interface in self._hardware.items(): diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index ce190b4f61..8fb85778e7 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -23,6 +23,7 @@ import pytest from dimos.control.components import HardwareComponent, HardwareType, make_joints +from dimos.control.coordinator import ControlCoordinator from dimos.control.hardware_interface import ConnectedHardware from dimos.control.task import ( ControlMode, @@ -188,6 +189,47 @@ def test_write_command(self, connected_hardware, mock_adapter): mock_adapter.write_joint_positions.assert_called() +class TestControlCoordinatorLifecycle: + def test_start_stop_calls_adapter_activate_and_deactivate(self): + from dimos.hardware.manipulators.mock.adapter import MockAdapter + from dimos.hardware.manipulators.registry import adapter_registry + + class LifecycleAdapter(MockAdapter): + events: list[str] = [] + + def connect(self) -> bool: + self.events.append("connect") + return super().connect() + + def activate(self) -> bool: + self.events.append("activate") + return self.write_enable(True) + + def deactivate(self) -> bool: + self.events.append("deactivate") + return self.write_stop() + + def disconnect(self) -> None: + self.events.append("disconnect") + super().disconnect() + + adapter_registry.register("lifecycle_test", LifecycleAdapter) + component = HardwareComponent( + hardware_id="arm", + hardware_type=HardwareType.MANIPULATOR, + joints=make_joints("arm", 6), + adapter_type="lifecycle_test", + ) + coordinator = ControlCoordinator(publish_joint_state=False, hardware=[component]) + + try: + coordinator.start() + finally: + coordinator.stop() + + assert LifecycleAdapter.events == ["connect", "activate", "deactivate", "disconnect"] + + class TestJointTrajectoryTask: def test_initial_state(self, trajectory_task): assert trajectory_task.name == "test_traj" diff --git a/dimos/hardware/manipulators/spec.py b/dimos/hardware/manipulators/spec.py index 868b714bfa..3955416653 100644 --- a/dimos/hardware/manipulators/spec.py +++ b/dimos/hardware/manipulators/spec.py @@ -105,6 +105,14 @@ def is_connected(self) -> bool: """Check if connected.""" ... + def activate(self) -> bool: + """Prepare hardware for commanded motion after connect().""" + ... + + def deactivate(self) -> bool: + """Gracefully stop commanded motion before disconnect().""" + ... + def get_info(self) -> ManipulatorInfo: """Get manipulator info (vendor, model, DOF).""" ... diff --git a/dimos/hardware/manipulators/xarm/adapter.py b/dimos/hardware/manipulators/xarm/adapter.py index 3e24c530d1..cf99786d63 100644 --- a/dimos/hardware/manipulators/xarm/adapter.py +++ b/dimos/hardware/manipulators/xarm/adapter.py @@ -39,6 +39,10 @@ MM_TO_M = 0.001 M_TO_MM = 1000.0 MAX_CARTESIAN_SPEED_MM = 500.0 # Max cartesian speed in mm/s +_XARM_LIFECYCLE_SPEED_DEG = 20.0 +_XARM_LIFECYCLE_ACCEL_DEG = 500.0 +_XARM6_INITIAL_JOINTS_RAD = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +_XARM7_INITIAL_JOINTS_RAD = [0.0, 0.0, 0.0, 0.0, 0.0, -0.7, 0.0] # XArm mode codes _XARM_MODE_POSITION = 0 @@ -223,6 +227,63 @@ def write_joint_positions( code: int = self._arm.set_servo_angle_j(angles, speed=100, mvacc=500) return code == 0 + def activate(self) -> bool: + """Enable motion and move the arm to its initial joint pose.""" + if not self._arm: + return False + + self._prepare_for_position_motion() + if not self._move_to_initial_pose(): + return False + return self.set_control_mode(ControlMode.SERVO_POSITION) + + def deactivate(self) -> bool: + """Move the arm to its initial joint pose and enter stopped state.""" + if not self._arm: + return False + + self._prepare_for_position_motion() + if not self._move_to_initial_pose(): + return False + code: int = self._arm.set_state(4) + return code == 0 + + def _move_to_initial_pose(self) -> bool: + if not self._arm: + return False + + joints = self._initial_joints() + if joints is None: + return True + + code: int = self._arm.set_servo_angle( + angle=[math.degrees(joint) for joint in joints], + speed=_XARM_LIFECYCLE_SPEED_DEG, + mvacc=_XARM_LIFECYCLE_ACCEL_DEG, + wait=True, + ) + return code == 0 + + def _initial_joints(self) -> list[float] | None: + if self._dof == 6: + return _XARM6_INITIAL_JOINTS_RAD + if self._dof == 7: + return _XARM7_INITIAL_JOINTS_RAD + return None + + def _prepare_for_position_motion(self) -> None: + if not self._arm: + return + + if self._arm.warn_code != 0: + self._arm.clean_warn() + if self._arm.error_code != 0: + self._arm.clean_error() + self._arm.motion_enable(enable=True) + self._arm.set_mode(_XARM_MODE_POSITION) + self._arm.set_state(0) + self._control_mode = ControlMode.POSITION + def write_joint_velocities(self, velocities: list[float]) -> bool: """Write joint velocities (rad/s -> deg/s). From 126ac0e2b755b15c9a04be3a862109c9f5dba4a1 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 3 Jun 2026 17:07:19 -0700 Subject: [PATCH 3/6] fix: xarm initial joints in degrees and motion enable on stop - Fix _XARM6/7_INITIAL_JOINTS to use degrees instead of radians - Add motion_enable(False) before set_state(4) in stop() - Update custom arm docs with activate/deactivate lifecycle methods - Ignore .omo/ directory --- .gitignore | 2 ++ dimos/hardware/manipulators/README.md | 1 + dimos/hardware/manipulators/xarm/adapter.py | 16 +++++++++------- .../manipulation/adding_a_custom_arm.md | 16 +++++++++++++--- 4 files changed, 25 insertions(+), 10 deletions(-) diff --git a/.gitignore b/.gitignore index ffc1c4f31f..a39e440ecc 100644 --- a/.gitignore +++ b/.gitignore @@ -60,6 +60,7 @@ yolo11n.pt .envrc .claude **/CLAUDE.md +.omo/ .direnv/ /logs @@ -89,3 +90,4 @@ recording*.db # Rerun recordings *.rrd + diff --git a/dimos/hardware/manipulators/README.md b/dimos/hardware/manipulators/README.md index 08c679726e..35e6b710f0 100644 --- a/dimos/hardware/manipulators/README.md +++ b/dimos/hardware/manipulators/README.md @@ -124,6 +124,7 @@ All adapters must implement these core methods: | Category | Methods | |----------|---------| | Connection | `connect()`, `disconnect()`, `is_connected()` | +| Lifecycle | `activate()`, `deactivate()` | | Info | `get_info()`, `get_dof()`, `get_limits()` | | State | `read_joint_positions()`, `read_joint_velocities()`, `read_joint_efforts()` | | Motion | `write_joint_positions()`, `write_joint_velocities()`, `write_stop()` | diff --git a/dimos/hardware/manipulators/xarm/adapter.py b/dimos/hardware/manipulators/xarm/adapter.py index cf99786d63..bfc96d398f 100644 --- a/dimos/hardware/manipulators/xarm/adapter.py +++ b/dimos/hardware/manipulators/xarm/adapter.py @@ -41,8 +41,9 @@ MAX_CARTESIAN_SPEED_MM = 500.0 # Max cartesian speed in mm/s _XARM_LIFECYCLE_SPEED_DEG = 20.0 _XARM_LIFECYCLE_ACCEL_DEG = 500.0 -_XARM6_INITIAL_JOINTS_RAD = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -_XARM7_INITIAL_JOINTS_RAD = [0.0, 0.0, 0.0, 0.0, 0.0, -0.7, 0.0] +_XARM6_INITIAL_JOINTS_DEG = [0.0, -40.0, -50.0, 0.0, 90.0, 0.0] +# TODO (CC): change this once we have 7dof arm setup +_XARM7_INITIAL_JOINTS_DEG = [0.0, 0.0, 0.0, 0.0, 0.0, math.degrees(-0.7), 0.0] # XArm mode codes _XARM_MODE_POSITION = 0 @@ -245,6 +246,7 @@ def deactivate(self) -> bool: self._prepare_for_position_motion() if not self._move_to_initial_pose(): return False + self._arm.motion_enable(enable=False) code: int = self._arm.set_state(4) return code == 0 @@ -252,23 +254,23 @@ def _move_to_initial_pose(self) -> bool: if not self._arm: return False - joints = self._initial_joints() + joints = self._initial_joints_degrees() if joints is None: return True code: int = self._arm.set_servo_angle( - angle=[math.degrees(joint) for joint in joints], + angle=joints, speed=_XARM_LIFECYCLE_SPEED_DEG, mvacc=_XARM_LIFECYCLE_ACCEL_DEG, wait=True, ) return code == 0 - def _initial_joints(self) -> list[float] | None: + def _initial_joints_degrees(self) -> list[float] | None: if self._dof == 6: - return _XARM6_INITIAL_JOINTS_RAD + return _XARM6_INITIAL_JOINTS_DEG if self._dof == 7: - return _XARM7_INITIAL_JOINTS_RAD + return _XARM7_INITIAL_JOINTS_DEG return None def _prepare_for_position_motion(self) -> None: diff --git a/docs/capabilities/manipulation/adding_a_custom_arm.md b/docs/capabilities/manipulation/adding_a_custom_arm.md index 0fd27b4e46..1e0a14a2a0 100644 --- a/docs/capabilities/manipulation/adding_a_custom_arm.md +++ b/docs/capabilities/manipulation/adding_a_custom_arm.md @@ -141,6 +141,14 @@ class YourArmAdapter: """Check if connected.""" return self._sdk is not None and self._sdk.is_alive() + def activate(self) -> bool: + """Prepare hardware for commanded motion after connect().""" + return self.write_enable(True) + + def deactivate(self) -> bool: + """Gracefully stop commanded motion before disconnect().""" + return self.write_stop() + def get_info(self) -> ManipulatorInfo: """Get manipulator info (vendor, model, DOF).""" @@ -632,6 +640,8 @@ def mock_adapter(): adapter.read_joint_velocities.return_value = [0.0] * 6 adapter.read_joint_efforts.return_value = [0.0] * 6 adapter.write_joint_positions.return_value = True + adapter.activate.return_value = True + adapter.deactivate.return_value = True adapter.read_enabled.return_value = True adapter.is_connected.return_value = True return adapter @@ -674,12 +684,12 @@ positions = adapter.read_joint_positions() assert len(positions) == 6 print(f"Joint positions (rad): {positions}") -# Enable and move -adapter.write_enable(True) +# Activate and move +adapter.activate() adapter.write_joint_positions([0.0] * 6) # Cleanup -adapter.write_stop() +adapter.deactivate() adapter.disconnect() ``` From d33a47125b48bf9445b95564336a910a4060326b Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 3 Jun 2026 17:37:34 -0700 Subject: [PATCH 4/6] refactor: simplify activate/deactivate calls in coordinator Remove unnecessary getattr/callable/hasattr guards since ManipulatorAdapter Protocol guarantees these methods exist. --- dimos/control/coordinator.py | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 6512d77b8d..25ea0500a3 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -225,12 +225,8 @@ def _setup_hardware(self, component: HardwareComponent) -> None: try: if component.auto_enable: - activate = getattr(adapter, "activate", None) - if callable(activate): - if activate() is False: - raise RuntimeError(f"Failed to activate hardware {component.hardware_id}") - elif hasattr(adapter, "write_enable"): - adapter.write_enable(True) + if adapter.activate() is False: + raise RuntimeError(f"Failed to activate hardware {component.hardware_id}") self.add_hardware(adapter, component) except Exception: @@ -713,13 +709,11 @@ def stop(self) -> None: with self._hardware_lock: for hw_id, interface in self._hardware.items(): - deactivate = getattr(interface.adapter, "deactivate", None) - if callable(deactivate): - try: - if deactivate() is False: - logger.error(f"Hardware {hw_id} deactivate returned False") - except Exception as e: - logger.error(f"Error deactivating hardware {hw_id}: {e}") + try: + if interface.adapter.deactivate() is False: + logger.error(f"Hardware {hw_id} deactivate returned False") + except Exception as e: + logger.error(f"Error deactivating hardware {hw_id}: {e}") # Disconnect all hardware adapters with self._hardware_lock: From 7be05cd1c35843b546a5a96ee757789a1c5bd949 Mon Sep 17 00:00:00 2001 From: cc Date: Wed, 3 Jun 2026 18:15:36 -0700 Subject: [PATCH 5/6] fix: address lifecycle review feedback - Guard activate()/deactivate() calls in the coordinator so adapters without lifecycle methods (twist bases, whole-body) no longer raise AttributeError; restore the write_enable(True) fallback on setup - Implement activate()/deactivate() in MockAdapter and ShmMujocoAdapter to satisfy the extended ManipulatorAdapter protocol - Log and set the stop event when keyboard teleop fails to read the initial joint state instead of exiting the thread silently - Remove unused home_pose computation in keyboard teleop - Add coordinator test covering adapters without lifecycle methods Co-Authored-By: Claude Opus 4.8 (1M context) --- dimos/control/coordinator.py | 13 +++++++--- dimos/control/test_control.py | 24 +++++++++++++++++++ dimos/hardware/manipulators/mock/adapter.py | 9 +++++++ dimos/hardware/manipulators/sim/adapter.py | 7 ++++++ .../teleop/keyboard/keyboard_teleop_module.py | 9 ++++++- 5 files changed, 58 insertions(+), 4 deletions(-) diff --git a/dimos/control/coordinator.py b/dimos/control/coordinator.py index 25ea0500a3..533e3b240e 100644 --- a/dimos/control/coordinator.py +++ b/dimos/control/coordinator.py @@ -225,8 +225,12 @@ def _setup_hardware(self, component: HardwareComponent) -> None: try: if component.auto_enable: - if adapter.activate() is False: - raise RuntimeError(f"Failed to activate hardware {component.hardware_id}") + activate = getattr(adapter, "activate", None) + if callable(activate): + if activate() is False: + raise RuntimeError(f"Failed to activate hardware {component.hardware_id}") + elif hasattr(adapter, "write_enable"): + adapter.write_enable(True) self.add_hardware(adapter, component) except Exception: @@ -709,8 +713,11 @@ def stop(self) -> None: with self._hardware_lock: for hw_id, interface in self._hardware.items(): + deactivate = getattr(interface.adapter, "deactivate", None) + if not callable(deactivate): + continue try: - if interface.adapter.deactivate() is False: + if deactivate() is False: logger.error(f"Hardware {hw_id} deactivate returned False") except Exception as e: logger.error(f"Error deactivating hardware {hw_id}: {e}") diff --git a/dimos/control/test_control.py b/dimos/control/test_control.py index 8fb85778e7..ae6bc1e9de 100644 --- a/dimos/control/test_control.py +++ b/dimos/control/test_control.py @@ -229,6 +229,30 @@ def disconnect(self) -> None: assert LifecycleAdapter.events == ["connect", "activate", "deactivate", "disconnect"] + def test_start_stop_with_adapter_without_lifecycle_methods(self): + """Adapters without activate/deactivate (e.g. twist bases) start and stop cleanly.""" + from dimos.control.components import make_twist_base_joints + + component = HardwareComponent( + hardware_id="base", + hardware_type=HardwareType.BASE, + joints=make_twist_base_joints("base"), + adapter_type="mock_twist_base", + ) + coordinator = ControlCoordinator(publish_joint_state=False, hardware=[component]) + + try: + coordinator.start() + adapter = coordinator._hardware["base"].adapter + assert not hasattr(adapter, "activate") + assert not hasattr(adapter, "deactivate") + # auto_enable falls back to write_enable(True) for adapters without activate() + assert adapter.read_enabled() + finally: + coordinator.stop() + + assert not adapter.is_connected() + class TestJointTrajectoryTask: def test_initial_state(self, trajectory_task): diff --git a/dimos/hardware/manipulators/mock/adapter.py b/dimos/hardware/manipulators/mock/adapter.py index f80ec0b57f..95778f1b49 100644 --- a/dimos/hardware/manipulators/mock/adapter.py +++ b/dimos/hardware/manipulators/mock/adapter.py @@ -81,6 +81,15 @@ def is_connected(self) -> bool: """Check mock connection status.""" return self._connected + def activate(self) -> bool: + """Simulate activation (enable servos).""" + return self.write_enable(True) + + def deactivate(self) -> bool: + """Simulate deactivation (stop motion, disable servos).""" + self.write_stop() + return self.write_enable(False) + def get_info(self) -> ManipulatorInfo: """Return mock info.""" return ManipulatorInfo( diff --git a/dimos/hardware/manipulators/sim/adapter.py b/dimos/hardware/manipulators/sim/adapter.py index 90d9dbb1aa..0640461ff8 100644 --- a/dimos/hardware/manipulators/sim/adapter.py +++ b/dimos/hardware/manipulators/sim/adapter.py @@ -119,6 +119,13 @@ def disconnect(self) -> None: def is_connected(self) -> bool: return self._connected and self._shm is not None + def activate(self) -> bool: + return self.write_enable(True) + + def deactivate(self) -> bool: + self.write_stop() + return self.write_enable(False) + def get_info(self) -> ManipulatorInfo: return ManipulatorInfo( vendor="Simulation", diff --git a/dimos/teleop/keyboard/keyboard_teleop_module.py b/dimos/teleop/keyboard/keyboard_teleop_module.py index 6cb85d3dba..804d6c1e87 100644 --- a/dimos/teleop/keyboard/keyboard_teleop_module.py +++ b/dimos/teleop/keyboard/keyboard_teleop_module.py @@ -48,6 +48,9 @@ from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.sensor_msgs.JointState import JointState +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() # Force X11 driver to avoid OpenGL threading issues os.environ["SDL_VIDEODRIVER"] = "x11" @@ -136,8 +139,12 @@ def _pygame_loop(self) -> None: initial_joints = self._read_joint_positions(self.config.initial_state_timeout) if initial_joints is None: + logger.error( + f"Failed to read initial joint state within " + f"{self.config.initial_state_timeout}s; keyboard teleop exiting" + ) + self._stop_event.set() return - home_pose = JogState.from_fk(model_path, ee_joint_id, self.config.home_joints) current_pose = JogState.from_fk(model_path, ee_joint_id, initial_joints).copy() # Publish initial pose From 2d8754712b8a33a29ba385a1564e738ddb653e36 Mon Sep 17 00:00:00 2001 From: cc Date: Mon, 8 Jun 2026 17:02:24 -0700 Subject: [PATCH 6/6] fix: add manipulator adapter lifecycle methods --- dimos/hardware/manipulators/a750/adapter.py | 8 ++ .../hardware/manipulators/openarm/adapter.py | 8 ++ dimos/hardware/manipulators/piper/adapter.py | 8 ++ .../manipulators/test_adapter_lifecycle.py | 132 ++++++++++++++++++ 4 files changed, 156 insertions(+) create mode 100644 dimos/hardware/manipulators/test_adapter_lifecycle.py diff --git a/dimos/hardware/manipulators/a750/adapter.py b/dimos/hardware/manipulators/a750/adapter.py index 904388e506..1a04f8230c 100644 --- a/dimos/hardware/manipulators/a750/adapter.py +++ b/dimos/hardware/manipulators/a750/adapter.py @@ -98,6 +98,14 @@ def is_connected(self) -> bool: self._connected = bool(self._robot.is_connected()) return self._connected + def activate(self) -> bool: + return self.write_enable(True) + + def deactivate(self) -> bool: + stopped = self.write_stop() + disabled = self.write_enable(False) + return stopped and disabled + def get_info(self) -> ManipulatorInfo: """Get A-750 information.""" self._trace("get_info") diff --git a/dimos/hardware/manipulators/openarm/adapter.py b/dimos/hardware/manipulators/openarm/adapter.py index 70554817e4..4a00556486 100644 --- a/dimos/hardware/manipulators/openarm/adapter.py +++ b/dimos/hardware/manipulators/openarm/adapter.py @@ -203,6 +203,14 @@ def disconnect(self) -> None: def is_connected(self) -> bool: return self._bus is not None + def activate(self) -> bool: + return self.write_enable(True) + + def deactivate(self) -> bool: + stopped = self.write_stop() + disabled = self.write_enable(False) + return stopped and disabled + def get_info(self) -> ManipulatorInfo: return ManipulatorInfo( vendor="Enactic", diff --git a/dimos/hardware/manipulators/piper/adapter.py b/dimos/hardware/manipulators/piper/adapter.py index 49ed68bcf9..f1f1cdcd77 100644 --- a/dimos/hardware/manipulators/piper/adapter.py +++ b/dimos/hardware/manipulators/piper/adapter.py @@ -135,6 +135,14 @@ def is_connected(self) -> bool: except Exception: return False + def activate(self) -> bool: + return self.write_enable(True) + + def deactivate(self) -> bool: + stopped = self.write_stop() + disabled = self.write_enable(False) + return stopped and disabled + def get_info(self) -> ManipulatorInfo: """Get Piper information.""" firmware_version = None diff --git a/dimos/hardware/manipulators/test_adapter_lifecycle.py b/dimos/hardware/manipulators/test_adapter_lifecycle.py new file mode 100644 index 0000000000..a955261aa9 --- /dev/null +++ b/dimos/hardware/manipulators/test_adapter_lifecycle.py @@ -0,0 +1,132 @@ +# 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. + +from __future__ import annotations + +from typing_extensions import override + +from dimos.hardware.manipulators.a750.adapter import A750Adapter +from dimos.hardware.manipulators.openarm.adapter import OpenArmAdapter +from dimos.hardware.manipulators.piper.adapter import PiperAdapter + + +class _PiperSdk: + def __init__(self) -> None: + self.actions: list[str] = [] + + def EnablePiper(self) -> bool: + self.actions.append("enable") + return True + + def MotionCtrl_2(self, **_: object) -> None: + self.actions.append("position_mode") + + def EmergencyStop(self) -> None: + self.actions.append("stop") + + def DisablePiper(self) -> None: + self.actions.append("disable") + + +class _LifecyclePiperAdapter(PiperAdapter): + def use_sdk(self, sdk: _PiperSdk) -> None: + self._sdk: _PiperSdk | None + self._sdk = sdk + + +def test_piper_lifecycle_enables_then_stops_and_disables() -> None: + sdk = _PiperSdk() + adapter = _LifecyclePiperAdapter() + adapter.use_sdk(sdk) + + assert adapter.activate() + assert adapter.deactivate() + assert sdk.actions == ["enable", "position_mode", "stop", "disable"] + + +class _OpenArmLifecycle: + def __init__(self) -> None: + self.actions: list[str] = [] + + def enable_all(self) -> None: + self.actions.append("enable") + + def disable_all(self) -> None: + self.actions.append("disable") + + +class _LifecycleOpenArmAdapter(OpenArmAdapter): + def __init__(self, lifecycle: _OpenArmLifecycle) -> None: + super().__init__() + self._lifecycle: _OpenArmLifecycle + self._lifecycle = lifecycle + + @override + def read_joint_positions(self) -> list[float]: + return [0.0] * 7 + + @override + def _compute_gravity_torques(self, q: list[float]) -> list[float]: + return [0.0] * len(q) + + @override + def write_enable(self, enable: bool) -> bool: + if enable: + self._lifecycle.enable_all() + else: + self._lifecycle.disable_all() + return True + + @override + def write_stop(self) -> bool: + self._lifecycle.actions.append("hold") + return True + + +def test_openarm_lifecycle_enables_then_holds_and_disables() -> None: + lifecycle = _OpenArmLifecycle() + adapter = _LifecycleOpenArmAdapter(lifecycle) + + assert adapter.activate() + assert adapter.deactivate() + assert lifecycle.actions == ["enable", "hold", "disable"] + + +class _A750Robot: + def __init__(self) -> None: + self.actions: list[str] = [] + + def start_control_loop(self) -> None: + self.actions.append("start") + + def stop_control_loop(self) -> None: + self.actions.append("stop") + + +class _LifecycleA750Adapter(A750Adapter): + def use_robot(self, robot: _A750Robot) -> None: + self._robot: _A750Robot | None + self._connected: bool + self._robot = robot + self._connected = True + + +def test_a750_lifecycle_starts_then_stops_control_loop() -> None: + robot = _A750Robot() + adapter = _LifecycleA750Adapter() + adapter.use_robot(robot) + + assert adapter.activate() + assert adapter.deactivate() + assert robot.actions == ["start", "stop"]