feat: xarm manipulator adapter improvement

.gitignore

@@ -60,6 +60,7 @@ yolo11n.pt
 .envrc
 .claude
 **/CLAUDE.md
+.omo/
 .direnv/
 
 /logs
@@ -89,3 +90,4 @@ recording*.db
 
 # Rerun recordings
 *.rrd
+

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,17 @@ 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 not callable(deactivate):
+                    continue
+                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():

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,71 @@ 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"]
+
+    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):
         assert trajectory_task.name == "test_traj"

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()` |

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(

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",

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

dimos/hardware/manipulators/xarm/adapter.py

@@ -39,6 +39,11 @@
 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_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
@@ -223,6 +228,64 @@ 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
+        self._arm.motion_enable(enable=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_degrees()
+        if joints is None:
+            return True
+
+        code: int = self._arm.set_servo_angle(
+            angle=joints,
+            speed=_XARM_LIFECYCLE_SPEED_DEG,
+            mvacc=_XARM_LIFECYCLE_ACCEL_DEG,
+            wait=True,
+        )
+        return code == 0
+
+    def _initial_joints_degrees(self) -> list[float] | None:
+        if self._dof == 6:
+            return _XARM6_INITIAL_JOINTS_DEG
+        if self._dof == 7:
+            return _XARM7_INITIAL_JOINTS_DEG
+        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).
 

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,