feat: xarm manipulator adapter improvement

[TomCC7]

Internal version of #2353 , closing the original pr.

Closes #1183

Solution

This PR resolves the following issues observed on teleoperating xarm:

KeyboardTeleopModule initial position sync

Wire robot joint states to the keyboard teleop module so that it initialize target to the robot startup position. A more proper fix in my mind should be to refactor keyboard teleop module to only output relative cartesian motion and wire that to some relative cartesian task so that the teleop module is decoupled from the actual robot state.

XArm graceful start/stop

Added two lifecycle methods in manipulator adapter activate/deactivate to execute functions required before robot starts/stop movement. The semantic is different from connect/disconnect in that sometime you might want to only pause robot commanding while still keeping the connection. Now the xarm will move to the default position on start/stop.

How to Test

# connects to a real robot for testing
dimos --xarm6-ip=192.168.1.210 run keyboard-teleop-xarm6

Contributor License Agreement

I have read and approved the CLA.

[greptile-apps]

Greptile Summary

This PR introduces activate()/deactivate() lifecycle methods across all manipulator adapters and wires them into the ControlCoordinator startup/shutdown path. It also replaces the keyboard teleop module's static home-pose initialisation with a live joint-state read so the operator always starts from the robot's actual position.

• Coordinator lifecycle: on start(), activate() is called after connect() and raises if it returns exactly False; on stop(), deactivate() is called before disconnect() for every registered adapter, with errors logged but not fatal.
• XArm graceful start/stop: activate() clears errors, moves to a hardcoded safe pose in position mode, then switches to servo mode; deactivate() mirrors this in reverse and disables motion.
• Keyboard teleop sync: the pygame loop now reads the first arriving JointState message (with a configurable timeout) to seed current_pose, and SPACE re-syncs to the live robot pose instead of a static home configuration.

Confidence Score: 4/5

Safe to merge with awareness of the open keyboard-teleop threading issues raised in earlier review rounds

The coordinator lifecycle wiring and adapter implementations are correct. The xarm adapter's _prepare_for_position_motion discards SDK return codes (flagged previously), and the keyboard teleop thread can outlive stop() when the initial joint-state read is still blocked — both are open items from earlier rounds. The changes are otherwise well-tested and the new lifecycle path is straightforward.

dimos/teleop/keyboard/keyboard_teleop_module.py (thread-lifetime window when initial_state_timeout > DEFAULT_THREAD_JOIN_TIMEOUT) and dimos/hardware/manipulators/xarm/adapter.py (_prepare_for_position_motion return-code handling)

Important Files Changed

Filename	Overview
dimos/hardware/manipulators/xarm/adapter.py	Adds activate()/deactivate() lifecycle methods that move xarm to an initial pose; _prepare_for_position_motion() return codes are discarded (previously flagged)
dimos/control/coordinator.py	Routes auto_enable through activate() when available; adds deactivate() loop at stop(); uses is False sentinel to distinguish explicit failure from None returns
dimos/teleop/keyboard/keyboard_teleop_module.py	Replaces static home-pose init with live joint state read; SPACE key syncs to current pose; dt inflation and thread-lifetime issues previously flagged
dimos/control/test_control.py	Adds lifecycle integration tests; class-level events list is shared across instances (previously flagged)
dimos/hardware/manipulators/xarm/test_adapter.py	New test file covering activate() action sequence and degree conversion; deactivate() path is not exercised
dimos/robot/manipulators/xarm/blueprints.py	Passes coordinator_joint_names to KeyboardTeleopModule so initial pose sync targets the right joints

Sequence Diagram

sequenceDiagram
    participant CC as ControlCoordinator
    participant XA as XArmAdapter
    participant KT as KeyboardTeleopModule

    CC->>XA: connect()
    Note over XA: TCP connect, set servo mode

    CC->>XA: activate()
    XA->>XA: _prepare_for_position_motion()
    Note over XA: clean errors, enable motion, set mode=0
    XA->>XA: "_move_to_initial_pose(wait=True)"
    Note over XA: set_servo_angle to default pose
    XA->>XA: set_control_mode(SERVO_POSITION)
    XA-->>CC: True

    CC->>CC: add_hardware() → start publishing JointState

    KT->>KT: "_read_joint_positions(timeout=5s)"
    CC-->>KT: JointState message
    KT->>KT: JogState.from_fk(initial_joints)

    loop Teleop loop
        KT->>CC: cartesian_command.publish(PoseStamped)
        KT->>KT: SPACE → _read_joint_positions(0.1s) → sync pose
    end

    CC->>XA: deactivate()
    XA->>XA: _prepare_for_position_motion()
    XA->>XA: "_move_to_initial_pose(wait=True)"
    XA->>XA: motion_enable(False) + set_state(4)
    XA-->>CC: True

    CC->>XA: disconnect()

Loading

_{Reviews (4): Last reviewed commit: "Merge remote-tracking branch 'origin/mai..." | Re-trigger Greptile}

[codecov]

❌ 1 Tests Failed:

Tests completed	Failed	Passed	Skipped
2148	1	2147	70

View the full list of 1 ❄️ flaky test(s)

dimos.e2e_tests.test_dimsim_spatial_memory::test_go_to_the_bed

Flake rate in main: 25.00% (Passed 3 times, Failed 1 times)

Stack Traces | 561s run time

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x712cf7f53dd0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x712cf7eaa020>
human_input = <function human_input.<locals>.send_human_input at 0x712cf7eaa200>
dim_sim = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x712cf76b8440>
explore_house = <function explore_house.<locals>.explore at 0x712cf7eaa700>

    @pytest.mark.self_hosted_large
    def test_go_to_the_bed(lcm_spy, start_blueprint, human_input, dim_sim, explore_house) -> None:
        start_blueprint(
            "run",
            "unitree-go2-agentic",
            simulator="dimsim",
        )
        lcm_spy.save_topic(".../McpClient/on_system_modules/res")
        lcm_spy.wait_for_saved_topic(".../McpClient/on_system_modules/res", timeout=1200.0)
    
        explore_house()
    
        human_input("go to the bed")
    
>       lcm_spy.wait_until_odom_position(-3.567, -1.332, threshold=2, timeout=180)

dim_sim    = <dimos.e2e_tests.dim_sim_client.DimSimClient object at 0x712cf76b8440>
explore_house = <function explore_house.<locals>.explore at 0x712cf7eaa700>
human_input = <function human_input.<locals>.send_human_input at 0x712cf7eaa200>
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x712cf7f53dd0>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x712cf7eaa020>

dimos/e2e_tests/test_dimsim_spatial_memory.py:32: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/e2e_tests/lcm_spy.py:182: in wait_until_odom_position
    self.wait_for_message_result(
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x712cf7eaa8e0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x712cf7f53dd0>
        threshold  = 2
        timeout    = 180
        x          = -3.567
        y          = -1.332
dimos/e2e_tests/lcm_spy.py:168: in wait_for_message_result
    self.wait_until(
        event      = <threading.Event at 0x712cf76b9520: unset>
        fail_message = 'Failed to get to position x=-3.567, y=-1.332'
        listener   = <function LcmSpy.wait_for_message_result.<locals>.listener at 0x712cf7ea9d00>
        predicate  = <function LcmSpy.wait_until_odom_position.<locals>.predicate at 0x712cf7eaa8e0>
        self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x712cf7f53dd0>
        timeout    = 180
        topic      = '/odom#geometry_msgs.PoseStamped'
        type       = <class 'dimos.msgs.geometry_msgs.PoseStamped.PoseStamped'>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x712cf7f53dd0>

    def wait_until(
        self,
        *,
        condition: Callable[[], bool],
        timeout: float,
        error_message: str,
        poll_interval: float = 0.1,
    ) -> None:
        start_time = time.time()
        while time.time() - start_time < timeout:
            if condition():
                return
            time.sleep(poll_interval)
>       raise TimeoutError(error_message)
E       TimeoutError: Failed to get to position x=-3.567, y=-1.332

condition  = <bound method Event.is_set of <threading.Event at 0x712cf76b9520: unset>>
error_message = 'Failed to get to position x=-3.567, y=-1.332'
poll_interval = 0.1
self       = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x712cf7f53dd0>
start_time = 1781211678.6941087
timeout    = 180

dimos/e2e_tests/lcm_spy.py:105: TimeoutError

To view more test analytics, go to the Test Analytics Dashboard
_{📋 Got 3 mins? Take this short survey to help us improve Test Analytics.}

[mustafab0]

👍