Realtime XArm driver for joint positiona and velocitt control by mustafab0 · Pull Request #702 · dimensionalOS/dimos

mustafab0 · 2025-10-27T19:03:27Z

This PR does the following:

Key Features

100Hz control loop for real-time position/velocity control
LCM pub/sub for distributed system integration
RPC methods for direct hardware control
Position mode (radians) and velocity mode (deg/s)
Component-based API: motion, kinematics, system, gripper control

The XArm driver has the following: Topics
Subscribed:

/xarm/joint_position_command - JointCommand (positions in radians)
/xarm/joint_velocity_command - JointCommand (velocities in deg/s)

Published:

/xarm/joint_states - JointState (100Hz)
/xarm/robot_state - RobotState (10Hz)
/xarm/ft_ext, /xarm/ft_raw - WrenchStamped (force/torque)

Testing

With Mock Hardware (No Physical Robot)

bash
# Unit tests with mocked xArm hardware
python tests/test_xarm_rt_driver.p

luqiang21 · 2025-11-12T04:09:31Z

dimos/hardware/manipulators/xarm/xarm_driver.py

+                        # NOTE: velocities are in degrees/second, not radians!
+                        # Always send velocity commands (velocity changes frequently)
+                        code = self.arm.vc_set_joint_velocity(
+                            joint_cmd, False, self.config.velocity_duration


Bug here: joint_cmd is in radian, but this call expects degrees.

@luqiang21 the xarm SDK uses radians when sending joint position commands,

But when sending velocity commands it expects inputs as deg/s

hence the difference here.

luqiang21 · 2025-11-12T04:16:45Z

dimos/hardware/manipulators/xarm/xarm_driver.py

+        self._stop_event = threading.Event()
+
+        # Subscription management
+        self._disposables = CompositeDisposable()


You should call this line in start() not here. Creating here will not allow the driver to receive command after a stop/start cycle.

Reason: stop() disposed CompositeDisposable(), when you call another start() , the new subscription will be added to an already-disposed CompositeDisposable().

dimos/hardware/manipulators/xarm/sample_trajectory_generator.py

luqiang21 · 2025-11-12T04:29:36Z

dimos/hardware/manipulators/xarm/xarm_driver.py

+                    timeout_logged = False
+
+                # Send command if available
+                if joint_cmd is not None and len(joint_cmd) == self.config.num_joints:


You should check readiness of the robot before sending command, otherwise commands may fail silently or cause error accumulation.

luqiang21 · 2025-11-12T04:33:46Z

dimos/hardware/manipulators/xarm/xarm_driver.py

+        except Exception as e:
+            logger.error(f"[{self.config.ip_address}] Error recovery failed: {e}")
+
+    def _xarm_is_ready_write(self) -> bool:


Variables _last_state, _last_mode, _last_not_ready are used but may not be initialized before first use
They are only set when conditions are met, but if _xarm_is_ready_write() is called before any state changes, these variables don't exist

luqiang21 · 2025-11-12T04:37:42Z

dimos/hardware/manipulators/xarm/xarm_driver.py

+                    if self.config.velocity_control:
+                        zero_vel = [0.0] * self.config.num_joints
+                        self.arm.vc_set_joint_velocity(
+                            zero_vel, True, self.config.velocity_duration


When command timeout occurs, zero velocity is sent with is_radian=True (line 944)
But the control loop uses is_radian=False for velocity commands (line 959)

You should make them consistent by changing this like to False as well

luqiang21 · 2025-11-12T04:51:32Z

dimos/hardware/manipulators/xarm/xarm_driver.py

+            # Enable servo mode if configured
+            if self.config.enable_on_start:
+                logger.info("Enabling servo mode (mode=1)...")
+                code = self.arm.set_mode(1)  # Servo mode


Velocity Mode Never Enabled

Problem:

_initialize_arm() always sets servo mode (set_mode(1)) and expects mode 1, even when self.config.velocity_control is True.

The control loop later sends vc_set_joint_velocity commands assuming mode 4, so the controller rejects every command (code 9).

Impact:

Critical: velocity-control deployments can never leave servo mode, so no velocities are accepted.

Fix:

Make _initialize_arm() check velocity_control like _attempt_error_recovery() does

@luqiang21 in my testing i found that the velocity control and position control both work using the SERVO Mode (1) and they are not exclusive to their specific modes.

@luqiang21 in my testing i found that the velocity control and position control both work using the SERVO Mode (1) and they are not exclusive to their specific modes.

Is it desired behavior?

luqiang21 · 2025-11-12T04:54:35Z

@mustafab0
can you commit the fixes you had last Friday into this PR as well?
Also, please add the instructions to install xarm lib

paul-nechifor · 2025-11-12T05:01:57Z

tests/test_xarm_rt_driver.py

+    return mock
+
+
+def test_basic_connection():


Shouldn't these be pytest tests? If they have to be executed manually, they are bound to break.

I tried building this with pytest last time and couldnt figure it out, so went with this basic thing. I have added a new pytest now.

paul-nechifor · 2025-11-12T05:06:57Z

dimos/hardware/manipulators/xarm/test_xarm_driver.py

+    # Check connection via RPC
+    logger.info("Checking connection via RPC...")
+    code, version = driver.get_version()
+    if code == 0:


This should be:

assert code == 0, "Failed to get firmware version"

paul-nechifor · 2025-11-12T05:10:01Z

dimos/hardware/manipulators/xarm/test_xarm_driver.py

+        logger.info(f"✓ Firmware version: {version}")
+    else:
+        logger.error(f"✗ Failed to get firmware version: code={code}")
+        dimos.stop()


You can use a fixture like this and not have to manually clean up:

@pytest.fixture def dimos_cluster(): dimos = core.start(1) try: yield dimos finally: dimos.stop()

paul-nechifor · 2025-11-12T05:13:28Z

dimos/hardware/manipulators/xarm/xarm_driver.py

+        self._stop_event = threading.Event()
+
+        # Subscription management
+        self._disposables = CompositeDisposable()


self._disposables already exists on Module.

paul-nechifor · 2025-11-12T05:18:51Z

dimos/hardware/manipulators/xarm/xarm_driver.py

+
+        logger.info(f"Robot state loop started at {self.config.robot_state_rate}Hz")
+
+        while self._running:


Do you need self._running if you have self._stop_event? Events are thread safe. This could just be

while not self._stop_event.is_set()

paul-nechifor · 2025-11-19T23:16:06Z

dimos/hardware/manipulators/xarm/README.md

+python test_xarm_driver.py --ip 192.168.1.235
+python interactive_control.py --ip 192.168.1.235


It would be nice to have a blueprint for these commands. Picking up arguments from environment variables or CLI is supported.

paul-nechifor · 2025-11-19T23:19:29Z

dimos/hardware/manipulators/xarm/README.md

+To test with real hardware using the current Alfred embodiment:
+
+1. **Turn on the Flowbase** (xArm controller)
+2. **SSH into dimensional-cpu-2:**


This is a bit overly specific to us. We should probably have internal docs somewhere else.

paul-nechifor · 2025-11-19T23:50:59Z

dimos/hardware/manipulators/xarm/components/state_queries.py

+    Component providing state query RPC methods for XArmDriver.
+
+    This component assumes the parent class has:
+    - self.arm: XArmAPI instance
+    - self.config: XArmDriverConfig instance
+    - self._joint_state_lock: threading.Lock
+    - self._joint_states_: Optional[JointState]
+    - self._robot_state_: Optional[RobotState]


I'm a bit unsure about the architecture here. It seems like XArmDriver is a single class which had it's methods moved into five mixins: MotionControlComponent, StateQueryComponent, SystemControlComponent, KinematicsComponent, GripperControlComponent.

I don't think they are real components as they are not used independently in other places and the dependencies are described in comments like here.

Having a single class would make it a bit harder to read, but it would make it more robust as you can depend on mypy for typechecking instead of comments.

Thanks for taking a look!

The reason for making the five mixins was essentially to keep the main driver file simpler. The thought process behind this is as follows.

For most manipulation tasks to move the robot we are going to give it joint position commands or joint velocity commands from the main control loop thread. The mixins/components are essentially all the features that the xarm SDK exposes for the arm. Most of these will not be used in "planned tasks" but they are necessary, for example clear error etc.

I debated putting all the mixins in the main driver file but it got too long.

But further more another motivation for breaking them into components is to strategically categories them such that it becomes a standard template for creating drivers for other arms as well. As piper arm SDK does not have all the features that xarm SDK has and vice versa, it is easy to make additional mixins or omit whatever isn't available, while the main driver's position and velocity control remains largely unchanged.

I completely agree with breaking large classes into reusable and interchangeable components, the problem is that mixins are not the best way to achieve that because they are not good at encapsulation and providing interfaces.

For example, instead of assuming that self.arm exists in the MotionControlComponent mixin, you can change it to an actual class that accepts arm

class XArmDriver: def __init__(...): self.arm = ... self.motion_control = MotionControlComponent(self.arm)

Currently, MotionControlComponent reaches into the internals of XArmDriver, for example by setting self._joint_cmd_. That's why I'm saying XArmDriver is really a single class split into multiple files. For components to be reusable, they need to have an established interface. This can be verified statically so we don't end up altering one implementation without the others.

But if you want to keep mixins, you can change this comment to a Protocol.

Something like:

class StateQueryHost(Protocol): arm: XArmAPI config: XArmDriverConfig _joint_state_lock: threading.Lock _joint_states_: JointState | None _robot_state_: RobotState | None

and then annotate the self arguments.

class StateQueryComponent: def get_joint_state(self: StateQueryHost) -> JointState | None:

I'm not saying it should be done now, though. This PR is quite large and I don't want to block you. It would be nice to do it in a subsequent PR though. :)

paul-nechifor · 2025-11-20T01:41:24Z

tests/test_xarm_driver_unit.py

+    assert len(position) == 6, "Position should have 6 values [x,y,z,roll,pitch,yaw]"
+    assert position[0] == 200.0, "X position should match mock"
+
+    driver.stop()


Stop is already called by the driver fixture. I don't think you have to call it in every test.

paul-nechifor · 2025-11-20T01:43:14Z

tests/test_xarm_driver_unit.py

+        pass  # Expected - no transport in unit test
+
+    driver.start()
+    time.sleep(1.0)


Is there a way to avoid the sleep by waiting for the specific event to happen?

The worst thing about sleep is that it takes the same amount of time no matter how many cores you have. :D

… xarm driver

…lishing after trajectory completion

… deprecated type annotations to use Python 3.10+ syntax

…reads running and stop event

The xArm SDK returns 7 joints even when init_axis=6 is specified, including the gripper as joint 7. This caused the trajectory controller to send 7-joint commands which were rejected by the driver. Fix: Truncate SDK joint data to num_joints in both new and old firmware paths, ensuring only the configured number of arm joints are published. This fixes the issue where trajectories wouldn't execute because commands had the wrong number of joints. Works for xarm5, xarm6, and xarm7. Generated with [Claude Code](https://claude.com/claude-code) Co-Authored-By: Claude <noreply@anthropic.com>

luqiang21 · 2025-12-05T06:04:17Z

Why all your commits' time changed? It will be hard for reviewers to know what you changed to resolve their comments.

spomichter · 2025-12-16T01:29:56Z

tests/test_xarm_rt_driver.py

@@ -0,0 +1,385 @@
+# Copyright 2025 Dimensional Inc.


This dir is deprecated so look at how unit tests implemented w pytset throughout the codebase.

mustafab0 · 2025-12-16T01:37:39Z

Why all your commits' time changed? It will be hard for reviewers to know what you changed to resolve their comments.

I did a rebase and things got messy. Sorry about that.

mustafab0 · 2025-12-16T21:30:16Z

Updated driiver architecture is found here: #831

mustafab0 requested review from alexlin2, leshy and spomichter October 27, 2025 19:03

spomichter changed the base branch from main to dev October 27, 2025 22:16

luqiang21 suggested changes Nov 12, 2025

View reviewed changes

paul-nechifor reviewed Nov 12, 2025

View reviewed changes

mustafab0 requested review from luqiang21 and paul-nechifor November 18, 2025 19:57

paul-nechifor reviewed Nov 19, 2025

View reviewed changes

paul-nechifor reviewed Nov 20, 2025

View reviewed changes

mustafab0 added 12 commits December 1, 2025 16:30

restructured hardware folder

830b2aa

Added spec for armdriver

819c3c7

added direnv dir to gitignore

2b352bd

xarm_driver initial design and message setup

c9ba547

added custom messages for robot state and joint commands

5a69033

simple rt xarm driver running

0b1ac16

added a simple interactive position and velocity trajectory generative

f97b495

updated readme and spec.py based on the current implementation of the…

50fc3e2

… xarm driver

refactored xarm driver with mixins

306826f

reduced velocity controller timeout

1e328f1

fixed race issue when changing modes

c9258b7

disabled debug logging

1d8a1a4

mustafab0 and others added 17 commits December 1, 2025 16:31

disabled position command publsihing and enabled velocity command pub…

8170a78

…lishing after trajectory completion

extended robot state variables

036ec3b

addded error code interpreter

b66ddb7

added all api calls as rpc calls

6d13205

udated readme

8fa51be

with error recovery

16c2ef1

test files to test w/ hw and w/o hw

46af817

addressed PR comments

cce7f72

replaced num of joints to arm type

1c0f9f5

modified readme for adding multicast

fcdcd45

CI code cleanup

1b2b56c

Made the xArm SDK import lazy to fix CI test failures and Updated all…

9d2c9e5

… deprecated type annotations to use Python 3.10+ syntax

Use xarm_driver from aa108b1 with base from 11b7c9b, reverted to 2 th…

a940ff0

…reads running and stop event

removed subscription management as module already does it

4b3d60a

removed running booolean onlu using thread safe threading event

3862c16

fix imports

dab26e8

ruff/mypy fixes

e7392b8

mustafab0 force-pushed the xarm-rt-driver branch from f32afab to e7392b8 Compare December 2, 2025 00:39

mustafab0 and others added 5 commits December 1, 2025 17:05

fixed conflict markers after rebase

4fbc086

Fix typing.List derecation

af203e1

added xarm sdk to pyproject.toml

fc81c91

Merge remote-tracking branch 'origin/dev' into xarm-rt-driver

4b13f18

spomichter requested changes Dec 16, 2025

View reviewed changes

mustafab0 closed this Dec 16, 2025

mustafab0 deleted the xarm-rt-driver branch December 16, 2025 21:30


		logger.info(f"Robot state loop started at {self.config.robot_state_rate}Hz")

		while self._running:

		python test_xarm_driver.py --ip 192.168.1.235
		python interactive_control.py --ip 192.168.1.235

Conversation

mustafab0 commented Oct 27, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

luqiang21 commented Nov 12, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

luqiang21 commented Dec 5, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mustafab0 commented Dec 16, 2025

Uh oh!

mustafab0 commented Dec 16, 2025

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants