feat: xarm manipulator adapter improvement#2425
Conversation
- 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
Remove unnecessary getattr/callable/hasattr guards since ManipulatorAdapter Protocol guarantees these methods exist.
- 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) <noreply@anthropic.com>
TomCC7
requested review from
leshy,
mustafab0,
paul-nechifor and
spomichter
as code owners
Greptile SummaryThis PR improves the xArm manipulator integration by adding
Confidence Score: 4/5Safe to merge with one thread-lifecycle fix in the keyboard teleop module. The coordinator lifecycle wiring and all adapter implementations are correct. The one concrete gap is in KeyboardTeleopModule: DEFAULT_THREAD_JOIN_TIMEOUT (2 s) is shorter than the newly introduced initial_state_timeout (5 s), and get_next blocks in an uninterruptible RxPY .run() call. If stop() is called before a joint state arrives, the join times out and super().stop() cleans up the module while the thread is still alive; a joint state arriving in the remaining window would cause the thread to publish on an already-torn-down stream. dimos/teleop/keyboard/keyboard_teleop_module.py - thread join / initial-state-timeout mismatch Important Files Changed
Sequence DiagramsequenceDiagram
participant CC as ControlCoordinator
participant XA as XArmAdapter
participant KT as KeyboardTeleopModule
Note over CC,XA: coordinator.start()
CC->>XA: connect()
XA-->>CC: True
CC->>XA: activate()
XA->>XA: _prepare_for_position_motion() mode 0
XA->>XA: "_move_to_initial_pose() wait=True"
XA->>XA: set_control_mode(SERVO_POSITION) mode 1
XA-->>CC: True
CC->>CC: publish joint_state 100Hz
Note over KT: module.start()
KT->>KT: "_read_joint_positions timeout=5s"
CC-->>KT: JointState
KT->>KT: JogState.from_fk(initial_joints)
KT->>KT: cartesian_command.publish(initial_pose)
loop teleop loop
KT->>CC: cartesian_command PoseStamped
CC->>XA: write_joint_positions
end
Note over CC,XA: coordinator.stop()
CC->>XA: deactivate()
XA->>XA: _prepare_for_position_motion() re-enable mode 0
XA->>XA: "_move_to_initial_pose() wait=True"
XA->>XA: motion_enable False + set_state 4
XA-->>CC: True
CC->>XA: disconnect()
|
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
| @@ -71,6 +75,8 @@ class KeyboardTeleopConfig(ModuleConfig): | |||
| ee_joint_id: int = 6 | |||
| task_name: str = "cartesian_ik_arm" | |||
| home_joints: list[float] | None = None | |||
There was a problem hiding this comment.
home_joints config field is now dead code
The home_joints field in KeyboardTeleopConfig is no longer referenced anywhere in the updated code — the initial pose now always comes from the live joint_state stream. Having an ignored config option is confusing for downstream users: they may try to set it expecting it to have an effect. Consider removing the field or adding a deprecation notice until existing callers are updated.
Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!
| return None | ||
|
|
||
| if joint_names := self.config.joint_names: | ||
| name_to_position = dict(zip(msg.name, msg.position, strict=False)) |
There was a problem hiding this comment.
zip with strict=False can silently produce a wrong name→position mapping
If a JointState message arrives with name and position lists of different lengths (e.g., a partial update), zip with strict=False silently truncates to the shorter side. The downstream name_to_position dict would then miss some entries, causing the any(name not in ...) guard below to return None, which the caller will treat as "no data" rather than "corrupt data." Using strict=True would surface this as a distinct, diagnosable error.
| name_to_position = dict(zip(msg.name, msg.position, strict=False)) | |
| name_to_position = dict(zip(msg.name, msg.position, strict=True)) |
| class LifecycleAdapter(MockAdapter): | ||
| events: list[str] = [] |
There was a problem hiding this comment.
events class variable is shared across all instances
events: list[str] = [] is a class-level mutable attribute. Every instance of LifecycleAdapter appends to the same list. In this particular test only one instance is created, so it passes today, but if a second adapter instance were ever created (e.g., in a retry path inside the coordinator), the assertion on line 230 would see duplicate entries and fail unexpectedly. Prefer initialising it in __init__ to make the list per-instance.
| 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 |
There was a problem hiding this comment.
_prepare_for_position_motion silently ignores all SDK return codes
All four calls — clean_warn(), clean_error(), motion_enable(), set_mode(), and set_state() — return error codes that are discarded. If any of these fail (e.g., the arm is mid-fault and clean_error returns non-zero), the subsequent _move_to_initial_pose call will send a position command into an arm that hasn't actually been re-enabled, and activate()/deactivate() will continue rather than surfacing the failure. Checking the return values of at least motion_enable and set_state would make failures diagnosable at their root cause.
Codecov Report❌ Patch coverage is 📢 Thoughts on this report? Let us know! |
github-actions
Bot
added
the
ready-to-merge
1 participant
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-xarm6Contributor License Agreement
I have read and approved the CLA.