hex-zmq-servers 1.0.0__tar.gz → 1.0.2__tar.gz

{hex_zmq_servers-1.0.0/hex_zmq_servers.egg-info → hex_zmq_servers-1.0.2}/PKG-INFO

@@ -1,9 +1,9 @@
 Metadata-Version: 2.4
 Name: hex_zmq_servers
-Version: 1.0.0
+Version: 1.0.2
 Summary: HEXFELLOW ZMQ Servers
-Author-email: Dong Zhaorui <joray.dong@hexfellow.com>
-Maintainer-email: jecjune <zejun.chen@hexfellow.com>, Dong Zhaorui <joray.dong@hexfellow.com>
+Author-email: Dong Zhaorui <dzr159@gmail.com>
+Maintainer-email: jecjune <zejun.chen@hexfellow.com>, Dong Zhaorui <dzr159@gmail.com>
 License-Expression: Apache-2.0
 Project-URL: Homepage, https://github.com/hexfellow/hex_zmq_servers
 Project-URL: Repository, https://github.com/hexfellow/hex_zmq_servers.git
@@ -23,7 +23,7 @@ Requires-Python: >=3.10
 Description-Content-Type: text/markdown
 License-File: LICENSE
 Requires-Dist: pyzmq>=27.0.1
-Requires-Dist: hex_device<1.4.0,>=1.3.14
+Requires-Dist: hex_device<1.4.0,>=1.3.17
 Requires-Dist: hex_robo_utils<0.3.0,>=0.2.0
 Requires-Dist: opencv-python>=4.2
 Provides-Extra: berxel
@@ -164,7 +164,7 @@ If you want to cite this project in your work, you can use the following BibTeX
 
 ```bibtex
 @software{hex_zmq_servers,
-  author    = {Dong, Zhaorui},
+  author    = {Dong, Zhaorui and Chen, Zejun},
   title     = {Hex ZMQ Servers: A ZeroMQ-Based Embodied AI Communication Framework},
   year      = {2025},
   publisher = {Zenodo},
@@ -186,3 +186,10 @@ Apache License 2.0. See [LICENSE](LICENSE).
 
 [![Star History Chart](https://api.star-history.com/svg?repos=hexfellow/hex_zmq_servers&type=Date)](https://star-history.com/#hexfellow/hex_zmq_servers&Date)
 
+---
+
+# 👥 Contributors
+
+<a href="https://github.com/hexfellow/hex_zmq_servers/graphs/contributors">
+    <img src="https://contrib.rocks/image?repo=hexfellow/hex_zmq_servers" />
+</a>

{hex_zmq_servers-1.0.0 → hex_zmq_servers-1.0.2}/README.md

@@ -121,7 +121,7 @@ If you want to cite this project in your work, you can use the following BibTeX
 
 ```bibtex
 @software{hex_zmq_servers,
-  author    = {Dong, Zhaorui},
+  author    = {Dong, Zhaorui and Chen, Zejun},
   title     = {Hex ZMQ Servers: A ZeroMQ-Based Embodied AI Communication Framework},
   year      = {2025},
   publisher = {Zenodo},
@@ -143,3 +143,10 @@ Apache License 2.0. See [LICENSE](LICENSE).
 
 [![Star History Chart](https://api.star-history.com/svg?repos=hexfellow/hex_zmq_servers&type=Date)](https://star-history.com/#hexfellow/hex_zmq_servers&Date)
 
+---
+
+# 👥 Contributors
+
+<a href="https://github.com/hexfellow/hex_zmq_servers/graphs/contributors">
+    <img src="https://contrib.rocks/image?repo=hexfellow/hex_zmq_servers" />
+</a>

{hex_zmq_servers-1.0.0 → hex_zmq_servers-1.0.2}/hex_zmq_servers/__init__.py

@@ -15,6 +15,7 @@ from .zmq_base import HexZMQDummyClient, HexZMQDummyServer
 
 from .robot import HexRobotBase, HexRobotClientBase, HexRobotServerBase
 from .robot import HexRobotDummy, HexRobotDummyClient, HexRobotDummyServer
+from .robot import HexRobotHello, HexRobotHelloClient, HexRobotHelloServer
 from .robot import HexRobotHexarm, HexRobotHexarmClient, HexRobotHexarmServer, HEXARM_URDF_PATH_DICT
 
 from .cam import HexCamBase, HexCamClientBase, HexCamServerBase
@@ -28,6 +29,7 @@ HEX_ZMQ_SERVERS_PATH_DICT = {
     "zmq_dummy": f"{file_dir}/zmq_base.py",
     "robot_dummy": f"{file_dir}/robot/dummy/robot_dummy_srv.py",
     "robot_hexarm": f"{file_dir}/robot/hexarm/robot_hexarm_srv.py",
+    "robot_hello": f"{file_dir}/robot/hello/robot_hello_srv.py",
     "cam_dummy": f"{file_dir}/cam/dummy/cam_dummy_srv.py",
     "cam_rgb": f"{file_dir}/cam/rgb/cam_rgb_srv.py",
 }
@@ -35,6 +37,7 @@ HEX_ZMQ_CONFIGS_PATH_DICT = {
     "zmq_dummy": f"{file_dir}/config/zmq_dummy.json",
     "robot_dummy": f"{file_dir}/config/robot_dummy.json",
     "robot_hexarm": f"{file_dir}/config/robot_hexarm.json",
+    "robot_hello": f"{file_dir}/config/robot_hello.json",
     "cam_dummy": f"{file_dir}/config/cam_dummy.json",
     "cam_rgb": f"{file_dir}/config/cam_rgb.json",
 }
@@ -77,6 +80,9 @@ __all__ = [
     "HexRobotDummy",
     "HexRobotDummyClient",
     "HexRobotDummyServer",
+    "HexRobotHello",
+    "HexRobotHelloClient",
+    "HexRobotHelloServer",
     "HexRobotHexarm",
     "HexRobotHexarmClient",
     "HexRobotHexarmServer",

hex_zmq_servers-1.0.2/hex_zmq_servers/config/robot_hello.json

@@ -0,0 +1,18 @@
+{
+    "net": {
+        "ip": "127.0.0.1",
+        "port": 12345,
+        "realtime_mode": false,
+        "deque_maxlen": 10,
+        "client_timeout_ms": 200,
+        "server_timeout_ms": 1000,
+        "server_num_workers": 4
+    },
+    "params": {
+        "device_ip": "172.18.8.161",
+        "device_port": 8439,
+        "control_hz": 1000,
+        "arm_type": "archer_y6",
+        "sens_ts": true
+    }
+}

{hex_zmq_servers-1.0.0 → hex_zmq_servers-1.0.2}/hex_zmq_servers/robot/__init__.py

@@ -8,6 +8,7 @@
 
 from .robot_base import HexRobotBase, HexRobotClientBase, HexRobotServerBase
 from .dummy import HexRobotDummy, HexRobotDummyClient, HexRobotDummyServer
+from .hello import HexRobotHello, HexRobotHelloClient, HexRobotHelloServer
 from .hexarm import HexRobotHexarm, HexRobotHexarmClient, HexRobotHexarmServer, HEXARM_URDF_PATH_DICT
 
 __all__ = [
@@ -24,6 +25,11 @@ __all__ = [
     "HexRobotDummyClient",
     "HexRobotDummyServer",
 
+    # hello
+    "HexRobotHello",
+    "HexRobotHelloClient",
+    "HexRobotHelloServer",
+
     # hexarm
     "HexRobotHexarm",
     "HexRobotHexarmClient",

hex_zmq_servers-1.0.2/hex_zmq_servers/robot/hello/__init__.py

@@ -0,0 +1,18 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-12
+################################################################
+
+from .robot_hello import HexRobotHello
+from .robot_hello_cli import HexRobotHelloClient
+from .robot_hello_srv import HexRobotHelloServer
+
+__all__ = [
+    # robot
+    "HexRobotHello",
+    "HexRobotHelloClient",
+    "HexRobotHelloServer",
+]

hex_zmq_servers-1.0.2/hex_zmq_servers/robot/hello/robot_hello.py

@@ -0,0 +1,312 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-14
+################################################################
+
+import time
+import threading
+import numpy as np
+from collections import deque
+
+from ..robot_base import HexRobotBase
+from ...zmq_base import (
+    hex_zmq_ts_now,
+    hex_zmq_ts_delta_ms,
+    HexRate,
+)
+from ...hex_launch import hex_log, HEX_LOG_LEVEL
+from hex_device import HexDeviceApi, Arm, Hands
+from hex_device.motor_base import CommandType
+
+ROBOT_CONFIG = {
+    "device_ip": "172.18.8.161",
+    "device_port": 8439,
+    "control_hz": 250,
+    "arm_type": "archer_y6",
+    "sens_ts": True,
+}
+
+HELLO_DEVICE_TYPE_DICT = {
+    # 6 dof
+    "archer_y6": 26,
+    "archer_d6y": 26,
+    "archer_l6y": 26,
+    "firefly_y6": 26,
+}
+
+
+class HexRobotHello(HexRobotBase):
+
+    def __init__(
+        self,
+        robot_config: dict = ROBOT_CONFIG,
+        realtime_mode: bool = False,
+    ):
+        HexRobotBase.__init__(self, realtime_mode)
+
+        try:
+            device_ip = robot_config["device_ip"]
+            device_port = robot_config["device_port"]
+            control_hz = robot_config["control_hz"]
+            arm_type = HELLO_DEVICE_TYPE_DICT[robot_config["arm_type"]]
+            self.__sens_ts = robot_config["sens_ts"]
+        except KeyError as ke:
+            missing_key = ke.args[0]
+            raise ValueError(
+                f"robot_config is not valid, missing key: {missing_key}")
+
+        # variables
+        # hex_arm variables
+        self.__hex_api: HexDeviceApi | None = None
+        self.__arm: Arm | None = None
+        self.__gripper: Hands | None = None
+
+        # buffer
+        self.__arm_state_buffer: dict | None = None
+        self.__gripper_state_buffer: dict | None = None
+
+        # open device
+        self.__hex_api = HexDeviceApi(
+            ws_url=f"ws://{device_ip}:{device_port}",
+            control_hz=control_hz,
+        )
+
+        # open arm
+        while self.__hex_api.find_device_by_robot_type(arm_type) is None:
+            print("\033[33mArm not found\033[0m")
+            time.sleep(1)
+        self.__arm = self.__hex_api.find_device_by_robot_type(arm_type)
+        self.__arm.start()
+
+        # try to open gripper
+        self.__gripper = self.__hex_api.find_optional_device_by_id(1)
+        if self.__gripper is None:
+            print("\033[33mGripper not found\033[0m")
+        self.__gripper.set_rgb_stripe_command([0] * 6, [255] * 6, [0] * 6)
+
+        # variables init
+        arm_dofs = len(self.__arm)
+        self._dofs = [arm_dofs]
+        self._limits = np.array(self.__arm.get_joint_limits()).reshape(
+            -1, 3, 2)
+        self.__motor_idx = {"robot_arm": np.arange(arm_dofs).tolist()}
+        if self.__gripper is not None:
+            gripper_dofs = len(self.__gripper)
+            self._dofs.append(gripper_dofs)
+            gripper_limits = np.array(
+                self.__gripper.get_joint_limits()).reshape(-1, 3, 2)
+            self._limits = np.concatenate([self._limits, gripper_limits],
+                                          axis=0)
+            self.__motor_idx["robot_gripper"] = (np.arange(gripper_dofs) +
+                                                 arm_dofs).tolist()
+
+        # modify variables
+        self._dofs = np.array(self._dofs)
+        self._dofs_sum = self._dofs.sum()
+        self._limits = np.ascontiguousarray(np.asarray(self._limits)).reshape(
+            self._dofs_sum, 3, 2)
+
+        # start work loop
+        self._working.set()
+
+    def work_loop(self, hex_queues: list[deque | threading.Event]):
+        states_queue = hex_queues[0]
+        rgbs_queue = hex_queues[1]
+        stop_event = hex_queues[2]
+
+        last_states_ts = hex_zmq_ts_now()
+        states_count = 0
+        cmds_count = 0
+        cmds = np.zeros((self._dofs_sum, 5))
+        last_rgbs_seq = -1
+        rate = HexRate(2000)
+        while self._working.is_set() and not stop_event.is_set():
+            # states
+            ts, states = self.__get_states()
+            if states is not None:
+                if hex_zmq_ts_delta_ms(ts, last_states_ts) > 1e-6:
+                    last_states_ts = ts
+                    states_queue.append((ts, states_count, states))
+                    states_count = (states_count + 1) % self._max_seq_num
+
+            # cmds
+            cmds_count += 1
+            if cmds_count >= 1000:
+                cmds_count = 0
+                self.__set_cmds(cmds)
+
+            # rgbs
+            rgbs_pack = None
+            try:
+                rgbs_pack = rgbs_queue[
+                    -1] if self._realtime_mode else rgbs_queue.popleft()
+            except IndexError:
+                pass
+            if rgbs_pack is not None:
+                ts, seq, rgbs = rgbs_pack
+                if seq != last_rgbs_seq:
+                    last_rgbs_seq = seq
+                    if hex_zmq_ts_delta_ms(hex_zmq_ts_now(), ts) < 200.0:
+                        self.__set_rgbs(rgbs)
+
+            # sleep
+            rate.sleep()
+
+        # close
+        self.close()
+
+    def __get_states(self) -> tuple[np.ndarray | None, dict | None]:
+        if self.__arm is None:
+            return None, None
+
+        # (arm_dofs, 3) # pos vel eff
+        if self.__arm_state_buffer is None:
+            self.__arm_state_buffer = self.__arm.get_simple_motor_status()
+
+        # (gripper_dofs, 3) # pos vel eff
+        if self.__gripper is not None and self.__gripper_state_buffer is None:
+            self.__gripper_state_buffer = self.__gripper.get_simple_motor_status(
+            )
+
+        arm_ready = self.__arm_state_buffer is not None
+        gripper_ready = self.__gripper is None or self.__gripper_state_buffer is not None
+        if arm_ready and gripper_ready:
+            arm_ts = self.__arm_state_buffer['ts']
+            gripper_ts = self.__gripper_state_buffer[
+                'ts'] if self.__gripper is not None else arm_ts
+
+            delta_ms = hex_zmq_ts_delta_ms(arm_ts, gripper_ts)
+            if np.fabs(delta_ms) < 1e-6:
+                pos = self.__arm_state_buffer['pos']
+                vel = self.__arm_state_buffer['vel']
+                eff = self.__arm_state_buffer['eff']
+
+                if self.__gripper is not None:
+                    pos = np.concatenate(
+                        [pos, self.__gripper_state_buffer['pos']])
+                    vel = np.concatenate(
+                        [vel, self.__gripper_state_buffer['vel']])
+                    eff = np.concatenate(
+                        [eff, self.__gripper_state_buffer['eff']])
+
+                state = np.array([pos, vel, eff]).T
+                self.__arm_state_buffer, self.__gripper_state_buffer = None, None
+                return arm_ts if self.__sens_ts else hex_zmq_ts_now(), state
+            elif delta_ms > 0.0:
+                self.__gripper_state_buffer = None
+                return None, None
+            else:
+                self.__arm_state_buffer = None
+                return None, None
+
+        return None, None
+
+    def __set_cmds(self, cmds: np.ndarray) -> bool:
+        # cmds: (n)
+        # [pos_0, ..., pos_n]
+        # cmds: (n, 2)
+        # [[pos_0, tor_0], ..., [pos_n, tor_n]]
+        # cmds: (n, 5)
+        # [[pos_0, vel_0, tor_0, kp_0, kd_0], ..., [pos_n, vel_n, tor_n, kp_n, kd_n]]
+        if self.__arm is None:
+            print("\033[91mArm not found\033[0m")
+            return False
+
+        if cmds.shape[0] < self._dofs_sum:
+            print(
+                "\033[91mThe length of joint_angles must be greater than or equal to the number of motors\033[0m"
+            )
+            return False
+        elif cmds.shape[0] > self._dofs_sum:
+            print(
+                f"\033[33mThe length of joint_angles is greater than the number of motors\033[0m"
+            )
+            cmds = cmds[:self._dofs_sum]
+
+        cmd_pos = None
+        tar_vel = np.zeros(self._dofs_sum)
+        cmd_tor = np.zeros(self._dofs_sum)
+        cmd_kp = np.zeros(self._dofs_sum)
+        cmd_kd = np.zeros(self._dofs_sum)
+        if len(cmds.shape) == 1:
+            cmd_pos = cmds
+        elif len(cmds.shape) == 2:
+            if cmds.shape[1] == 2:
+                cmd_pos = cmds[:, 0]
+                cmd_tor = cmds[:, 1]
+            elif cmds.shape[1] == 5:
+                cmd_pos = cmds[:, 0]
+                tar_vel = cmds[:, 1]
+                cmd_tor = cmds[:, 2]
+                cmd_kp = cmds[:, 3]
+                cmd_kd = cmds[:, 4]
+            else:
+                raise ValueError(f"The shape of cmds is invalid: {cmds.shape}")
+        else:
+            raise ValueError(f"The shape of cmds is invalid: {cmds.shape}")
+        tar_pos = self._apply_pos_limits(
+            cmd_pos,
+            self._limits[:, 0, 0],
+            self._limits[:, 0, 1],
+        )
+
+        # arm
+        arm_cmd = self.__arm.construct_mit_command(
+            tar_pos[self.__motor_idx["robot_arm"]],
+            tar_vel[self.__motor_idx["robot_arm"]],
+            cmd_tor[self.__motor_idx["robot_arm"]],
+            cmd_kp[self.__motor_idx["robot_arm"]],
+            cmd_kd[self.__motor_idx["robot_arm"]],
+        )
+        self.__arm.motor_command(CommandType.MIT, arm_cmd)
+
+        # gripper
+        if self.__gripper is not None:
+            gripper_cmd = self.__gripper.construct_mit_command(
+                tar_pos[self.__motor_idx["robot_gripper"]],
+                tar_vel[self.__motor_idx["robot_gripper"]],
+                cmd_tor[self.__motor_idx["robot_gripper"]],
+                cmd_kp[self.__motor_idx["robot_gripper"]],
+                cmd_kd[self.__motor_idx["robot_gripper"]],
+            )
+            self.__gripper.motor_command(CommandType.MIT, gripper_cmd)
+
+        return True
+
+    def __set_rgbs(self, rgbs: np.ndarray) -> bool:
+        if self.__gripper is None:
+            print("\033[91mGripper not found\033[0m")
+            return False
+
+        rgbs_int = rgbs.astype(int)
+        rgbs_shape = rgbs_int.shape
+        rgbs_dim = len(rgbs_shape)
+        if rgbs_dim == 1:
+            if rgbs_shape[0] != 3:
+                print("\033[91mThe shape of rgbs is invalid\033[0m")
+                return False
+            rgbs_int = rgbs_int.reshape(1, 3)
+            rgbs_int = np.tile(rgbs_int, (6, 1))
+        elif rgbs_dim == 2 and rgbs_shape[1] != 3:
+            print("\033[91mThe shape of rgbs is invalid\033[0m")
+            return False
+
+        self.__gripper.set_rgb_stripe_command(
+            rgbs_int[:, 0].tolist(),
+            rgbs_int[:, 1].tolist(),
+            rgbs_int[:, 2].tolist(),
+        )
+        return True
+
+    def close(self):
+        if not self._working.is_set():
+            return
+        self._working.clear()
+        self.__gripper.set_rgb_stripe_command([255] * 6, [0] * 6, [0] * 6)
+        time.sleep(0.2)
+        self.__arm.stop()
+        self.__hex_api.close()
+        hex_log(HEX_LOG_LEVEL["info"], "HexRobotHello closed")

hex_zmq_servers-1.0.2/hex_zmq_servers/robot/hello/robot_hello_cli.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-14
+################################################################
+
+from ..robot_base import HexRobotClientBase
+from ...zmq_base import hex_zmq_ts_now, HexRate
+
+import numpy as np
+from collections import deque
+
+NET_CONFIG = {
+    "ip": "127.0.0.1",
+    "port": 12345,
+    "realtime_mode": False,
+    "deque_maxlen": 10,
+    "client_timeout_ms": 200,
+    "server_timeout_ms": 1_000,
+    "server_num_workers": 4,
+}
+
+ROBOT_CONFIG = {
+    "device_ip": "172.18.8.161",
+    "device_port": 8439,
+    "control_hz": 250,
+    "sens_ts": True,
+}
+
+
+class HexRobotHelloClient(HexRobotClientBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+    ):
+        HexRobotClientBase.__init__(self, net_config)
+        self._rgbs_seq = 0
+        self._rgbs_queue = deque(maxlen=1)
+        self._wait_for_working()
+
+    def set_rgbs(self, rgbs: np.ndarray):
+        self._rgbs_queue.append(rgbs)
+
+    def _set_rgbs_inner(self, rgbs: np.ndarray) -> bool:
+        hdr, rgbs = self.request(
+            {
+                "cmd": "set_rgbs",
+                "ts": hex_zmq_ts_now(),
+                "args": self._rgbs_seq,
+            },
+            rgbs,
+        )
+        # print(f"set_rgbs seq: {self._rgbs_seq}")
+        try:
+            cmd = hdr["cmd"]
+            if cmd == "set_rgbs_ok":
+                self._rgbs_seq = (self._rgbs_seq + 1) % self._max_seq_num
+                return True
+            else:
+                return False
+        except KeyError:
+            print(f"\033[91m{hdr['cmd']} requires `cmd`\033[0m")
+            return False
+        except Exception as e:
+            print(f"\033[91mset_rgbs failed: {e}\033[0m")
+            return False
+
+    def _recv_loop(self):
+        rate = HexRate(2000)
+        while self._recv_flag:
+            hdr, states = self._get_states_inner()
+            if hdr is not None:
+                self._states_queue.append((hdr, states))
+
+            try:
+                rgbs = self._rgbs_queue[-1]
+                _ = self._set_rgbs_inner(rgbs)
+            except IndexError:
+                pass
+
+            rate.sleep()

hex_zmq_servers-1.0.2/hex_zmq_servers/robot/hello/robot_hello_srv.py

@@ -0,0 +1,114 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-14
+################################################################
+
+import numpy as np
+from collections import deque
+
+try:
+    from ..robot_base import HexRobotServerBase
+    from .robot_hello import HexRobotHello
+except (ImportError, ValueError):
+    import sys
+    from pathlib import Path
+    this_file = Path(__file__).resolve()
+    project_root = this_file.parents[3]
+    if str(project_root) not in sys.path:
+        sys.path.insert(0, str(project_root))
+    from hex_zmq_servers.robot.robot_base import HexRobotServerBase
+    from hex_zmq_servers.robot.hello.robot_hello import HexRobotHello
+
+NET_CONFIG = {
+    "ip": "127.0.0.1",
+    "port": 12345,
+    "realtime_mode": False,
+    "deque_maxlen": 10,
+    "client_timeout_ms": 200,
+    "server_timeout_ms": 1_000,
+    "server_num_workers": 4,
+}
+
+ROBOT_CONFIG = {
+    "device_ip": "172.18.8.161",
+    "device_port": 8439,
+    "control_hz": 250,
+    "sens_ts": True,
+}
+
+
+class HexRobotHelloServer(HexRobotServerBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+        params_config: dict = ROBOT_CONFIG,
+    ):
+        HexRobotServerBase.__init__(self, net_config)
+        self._rgbs_queue = deque(maxlen=1)
+        self._rgbs_seq = -1
+
+        # robot
+        self._device = HexRobotHello(params_config,
+                                     net_config.get("realtime_mode", False))
+
+    def work_loop(self):
+        try:
+            self._device.work_loop([
+                self._states_queue,
+                self._rgbs_queue,
+                self._stop_event,
+            ])
+        finally:
+            self._device.close()
+
+    def _process_request(self, recv_hdr: dict, recv_buf: np.ndarray):
+        if recv_hdr["cmd"] == "is_working":
+            return self.no_ts_hdr(recv_hdr, self._device.is_working()), None
+        elif recv_hdr["cmd"] == "seq_clear":
+            return self.no_ts_hdr(recv_hdr, self._seq_clear()), None
+        elif recv_hdr["cmd"] == "get_dofs":
+            dofs = self._device.get_dofs()
+            return self.no_ts_hdr(recv_hdr, dofs is not None), dofs
+        elif recv_hdr["cmd"] == "get_limits":
+            limits = self._device.get_limits()
+            return self.no_ts_hdr(recv_hdr, limits is not None), limits
+        elif recv_hdr["cmd"] == "get_states":
+            return self._get_states(recv_hdr)
+        elif recv_hdr["cmd"] == "set_rgbs":
+            return self.__set_rgbs(recv_hdr, recv_buf)
+        else:
+            raise ValueError(f"unknown command: {recv_hdr['cmd']}")
+
+    def __set_rgbs(self, recv_hdr: dict, recv_buf: np.ndarray):
+        seq = recv_hdr.get("args", None)
+        if self._seq_clear_flag:
+            self._seq_clear_flag = False
+            self._rgbs_seq = -1
+            return self.no_ts_hdr(recv_hdr, False), None
+
+        if seq is not None:
+            delta = (seq - self._rgbs_seq) % self._max_seq_num
+            if delta >= 0 and delta < 1e6:
+                self._rgbs_seq = seq
+                self._rgbs_queue.append((recv_hdr["ts"], seq, recv_buf))
+                return self.no_ts_hdr(recv_hdr, True), None
+            else:
+                return self.no_ts_hdr(recv_hdr, False), None
+        else:
+            return self.no_ts_hdr(recv_hdr, False), None
+
+
+if __name__ == "__main__":
+    import argparse, json
+    from hex_zmq_servers.zmq_base import hex_server_helper
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--cfg", type=str, required=True)
+    args = parser.parse_args()
+    cfg = json.loads(args.cfg)
+
+    hex_server_helper(cfg, HexRobotHelloServer)

{hex_zmq_servers-1.0.0 → hex_zmq_servers-1.0.2}/hex_zmq_servers/robot/hexarm/__init__.py

@@ -26,6 +26,8 @@ HEXARM_URDF_PATH_DICT = {
     f"{urdf_dir}/archer_y6/gp100_p050_handle.urdf",
     "archer_d6y_empty":
     f"{urdf_dir}/archer_d6y/empty.urdf",
+    "archer_d6y_gp80":
+    f"{urdf_dir}/archer_d6y/gp80.urdf",
     "archer_d6y_gp100":
     f"{urdf_dir}/archer_d6y/gp100.urdf",
     "archer_d6y_gp100_handle":