hex-zmq-servers 0.3.9__py3-none-any.whl

hex_zmq_servers/robot/gello/robot_gello.py

@@ -0,0 +1,366 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-12
+################################################################
+
+import os
+import subprocess
+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 dynamixel_sdk.group_sync_read import GroupSyncRead
+from dynamixel_sdk.group_sync_write import GroupSyncWrite
+from dynamixel_sdk.packet_handler import PacketHandler
+from dynamixel_sdk.port_handler import PortHandler
+from dynamixel_sdk.robotis_def import (
+    COMM_SUCCESS,
+    DXL_HIBYTE,
+    DXL_HIWORD,
+    DXL_LOBYTE,
+    DXL_LOWORD,
+)
+
+DYNAMIXEL_TORQUE_ENABLE_ADDR = 64
+DYNAMIXEL_TORQUE_ENABLE_VAL = 1
+DYNAMIXEL_TORQUE_DISABLE_VAL = 0
+DYNAMIXEL_GOAL_POSITION_ADDR = 116
+DYNAMIXEL_GOAL_POSITION_LEN = 4
+DYNAMIXEL_PRESENT_POSITION_ADDR = 132
+DYNAMIXEL_PRESENT_POSITION_LEN = 4
+
+ROBOT_CONFIG = {
+    "idxs": [0, 1, 2, 3, 4, 5, 6],
+    "invs": [1.0, -1.0, 1.0, 1.0, -1.0, -4.0],
+    "limits": [[[-2.7, 2.7], [-1.57, 2.09], [0, 3.14], [-1.57, 1.57],
+                [-1.57, 1.57], [-1.57, 1.57], [0.0, 1.0]]],
+    "device":
+    "/dev/ttyUSB0",
+    "baudrate":
+    115200,
+    "max_retries":
+    3,
+    "torque_enabled":
+    False,
+    "sens_ts":
+    True,
+}
+
+
+class HexRobotGello(HexRobotBase):
+
+    def __init__(
+        self,
+        robot_config: dict = ROBOT_CONFIG,
+        realtime_mode: bool = False,
+    ):
+        HexRobotBase.__init__(self, realtime_mode)
+
+        try:
+            self.__idxs = np.array(robot_config["idxs"])
+            self.__invs = np.array(robot_config["invs"])
+            self._limits = np.array(robot_config["limits"])
+            self.__device = robot_config["device"]
+            self.__baudrate = robot_config["baudrate"]
+            self.__max_retries = robot_config["max_retries"]
+            self.__torque_enabled = robot_config["torque_enabled"]
+            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
+        # gello variables
+        self.__servo_to_rad = np.pi / 2048
+        self.__port_handler = None
+        self.__packet_handler = None
+        self.__group_sync_read = None
+        self.__group_sync_write = None
+        self.__lock = threading.Lock()
+        # robot variables
+        self._dofs = [self.__idxs.shape[0]]
+
+        # open device
+        for attempt in range(self.__max_retries):
+            print(
+                f"Attempting to initialize Dynamixel driver (attempt {attempt + 1}/{self.__max_retries})"
+            )
+            if self.__try_open_device():
+                break
+            else:
+                time.sleep(1.0)
+
+        # start work loop
+        self._working.set()
+
+    def work_loop(self, hex_queues: list[deque | threading.Event]):
+        states_queue = hex_queues[0]
+        cmds_queue = hex_queues[1]
+        stop_event = hex_queues[2]
+
+        states_count = 0
+        last_cmds_seq = -1
+        rate = HexRate(1000)
+        while self._working.is_set() and not stop_event.is_set():
+            # states
+            ts, states = self.__get_states()
+            if states is not None:
+                states_queue.append((ts, states_count, states))
+                states_count = (states_count + 1) % self._max_seq_num
+
+            # cmds
+            cmds_pack = None
+            try:
+                cmds_pack = cmds_queue[
+                    -1] if self._realtime_mode else cmds_queue.popleft()
+            except IndexError:
+                pass
+            if cmds_pack is not None:
+                ts, seq, cmds = cmds_pack
+                if seq != last_cmds_seq:
+                    last_cmds_seq = seq
+                    if hex_zmq_ts_delta_ms(hex_zmq_ts_now(), ts) < 200.0:
+                        self.__set_cmds(cmds)
+
+            # sleep
+            rate.sleep()
+
+        # close
+        self.close()
+
+    def __get_states(self):
+        with self.__lock:
+            ts = hex_zmq_ts_now()
+            servo_values = np.zeros(self._dofs, dtype=int)
+            dxl_comm_result = self.__group_sync_read.txRxPacket()
+            if dxl_comm_result != COMM_SUCCESS:
+                print(f"warning, comm failed: {dxl_comm_result}")
+                return None, None
+            for i, dxl_id in enumerate(self.__idxs):
+                if self.__group_sync_read.isAvailable(
+                        dxl_id, DYNAMIXEL_PRESENT_POSITION_ADDR,
+                        DYNAMIXEL_PRESENT_POSITION_LEN):
+                    servo_value = self.__group_sync_read.getData(
+                        dxl_id, DYNAMIXEL_PRESENT_POSITION_ADDR,
+                        DYNAMIXEL_PRESENT_POSITION_LEN)
+                    servo_value = np.int32(np.uint32(servo_value))
+                    servo_values[i] = servo_value
+                else:
+                    raise RuntimeError(
+                        f"Failed to get joint angles for Dynamixel with ID {dxl_id}"
+                    )
+
+            rads = self._apply_pos_limits(
+                servo_values * self.__servo_to_rad * self.__invs,
+                self._limits[:, 0],
+                self._limits[:, 1],
+            )
+            return ts if self.__sens_ts else hex_zmq_ts_now(), rads
+
+    def __set_cmds(self, cmds: np.ndarray):
+        if len(cmds) != len(self.__idxs):
+            print(
+                "\033[91mThe length of joint_angles must match the number of servos\033[0m"
+            )
+            return False
+        if not self.__torque_enabled:
+            print("\033[91mTorque must be enabled to set joint angles\033[0m")
+            return False
+
+        for dxl_id, cmd_rad in zip(self.__idxs, cmds):
+            # Convert the angle to the appropriate value for the servo
+            position_value = int(cmd_rad / self.__servo_to_rad)
+
+            # Allocate goal position value into byte array
+            param_goal_position = [
+                DXL_LOBYTE(DXL_LOWORD(position_value)),
+                DXL_HIBYTE(DXL_LOWORD(position_value)),
+                DXL_LOBYTE(DXL_HIWORD(position_value)),
+                DXL_HIBYTE(DXL_HIWORD(position_value)),
+            ]
+
+            # Add goal position value to the Syncwrite parameter storage
+            dxl_addparam_result = self.__group_sync_write.addParam(
+                dxl_id, param_goal_position)
+            if not dxl_addparam_result:
+                print(
+                    f"\033[91mFailed to set joint angle for Dynamixel with ID {dxl_id}\033[0m"
+                )
+                return False
+
+        # Syncwrite goal position
+        dxl_comm_result = self.__group_sync_write.txPacket()
+        if dxl_comm_result != COMM_SUCCESS:
+            print(f"\033[91mFailed to syncwrite goal position\033[0m")
+            return False
+
+        # Clear syncwrite parameter storage
+        self.__group_sync_write.clearParam()
+
+        return True
+
+    def __try_open_device(self):
+        if not self.__check_device_availability():
+            print("Port is busy, attempting to free it...")
+            if not self.__kill_processes_using_device():
+                print("Failed to free device, trying to fix permissions...")
+                self.__fix_device_permissions()
+            time.sleep(2)
+
+        try:
+            self.__initialize_hardware()
+            print(
+                f"Successfully initialized Dynamixel driver on {self.__device}"
+            )
+            return True
+        except Exception as e:
+            print(f"Failed to initialize Dynamixel driver: {e}")
+            if "Permission denied" in str(e):
+                print(
+                    "Please add permission to the device. For more details, please refer to the \033[0;31mhttps://docs.hexfellow.com\033[0m"
+                )
+                return False
+            return False
+
+    def __check_device_availability(self):
+        try:
+            # Check if port exists
+            if not os.path.exists(self.__device):
+                print(f"Device {self.__device} does not exist")
+                return False
+
+            # Check for processes using the port
+            result = subprocess.run(["lsof", self.__device],
+                                    capture_output=True,
+                                    text=True)
+
+            if result.returncode == 0:
+                lines = result.stdout.strip().split("\n")
+                if len(lines) > 1:  # Header + processes
+                    print(
+                        f"Device {self.__device} is being used by other processes:"
+                    )
+                    for line in lines[1:]:
+                        print(f"  {line}")
+                    return False
+            return True
+        except Exception as e:
+            print(f"Error checking device availability: {e}")
+            return False
+
+    def __kill_processes_using_device(self):
+        try:
+            result = subprocess.run(
+                ["fuser", "-k", self.__device],
+                capture_output=True,
+                text=True,
+            )
+            if result.returncode == 0:
+                print(f"Killed processes using {self.__device}")
+                time.sleep(1)
+                return True
+            return False
+        except Exception as e:
+            print(f"Error killing processes: {e}")
+            return False
+
+    def __fix_device_permissions(self):
+        try:
+            result = subprocess.run(["sudo", "chmod", "666", self.__device],
+                                    capture_output=True,
+                                    text=True)
+            if result.returncode == 0:
+                print(f"Fixed permissions for {self.__device}")
+                return True
+            return False
+        except Exception as e:
+            print(f"Error fixing device permissions: {e}")
+            return False
+
+    def __initialize_hardware(self):
+        # Check and prepare port before connection
+        self.__prepare_device()
+
+        # Initialize the port handler, packet handler, and group sync read/write
+        self.__port_handler = PortHandler(self.__device)
+        self.__packet_handler = PacketHandler(2.0)
+        self.__group_sync_read = GroupSyncRead(
+            self.__port_handler,
+            self.__packet_handler,
+            DYNAMIXEL_PRESENT_POSITION_ADDR,
+            DYNAMIXEL_PRESENT_POSITION_LEN,
+        )
+        self.__group_sync_write = GroupSyncWrite(
+            self.__port_handler,
+            self.__packet_handler,
+            DYNAMIXEL_GOAL_POSITION_ADDR,
+            DYNAMIXEL_GOAL_POSITION_LEN,
+        )
+
+        # Open the port and set the baudrate
+        if not self.__port_handler.openPort():
+            raise RuntimeError("Failed to open the port")
+
+        if not self.__port_handler.setBaudRate(self.__baudrate):
+            raise RuntimeError(
+                f"Failed to change the baudrate, {self.__baudrate}")
+
+        # Add parameters for each Dynamixel servo to the group sync read
+        for dxl_id in self.__idxs:
+            if not self.__group_sync_read.addParam(dxl_id):
+                raise RuntimeError(
+                    f"Failed to add parameter for Dynamixel with ID {dxl_id}")
+
+        # Disable torque for each Dynamixel servo
+        if self.__torque_enabled:
+            try:
+                self.__hardware_set_torque_mode(self.__torque_enabled)
+            except Exception as e:
+                print(f"device: {self.__device}, {e}")
+
+    def __prepare_device(self):
+        if not self.__check_device_availability():
+            print(
+                f"Device {self.__device} is not available, attempting to fix..."
+            )
+            self.__kill_processes_using_device()
+            self.__fix_device_permissions()
+
+            # Check again after fixing
+            if not self.__check_device_availability():
+                print(f"Warning: Device {self.__device} may still have issues")
+
+    def __hardware_set_torque_mode(self, enable: bool):
+        torque_value = DYNAMIXEL_TORQUE_ENABLE_VAL if enable else DYNAMIXEL_TORQUE_DISABLE_VAL
+        with self.__lock:
+            for dxl_id in self.__idxs:
+                dxl_comm_result, dxl_error = self.__packet_handler.write1ByteTxRx(
+                    self.__port_handler, dxl_id, DYNAMIXEL_TORQUE_ENABLE_ADDR,
+                    torque_value)
+                if dxl_comm_result != COMM_SUCCESS or dxl_error != 0:
+                    print(dxl_comm_result)
+                    print(dxl_error)
+                    raise RuntimeError(
+                        f"Failed to set torque mode for Dynamixel with ID {dxl_id}"
+                    )
+
+        self.__torque_enabled = enable
+
+    def close(self):
+        if not self._working.is_set():
+            return
+        self._working.clear()
+        self.__port_handler.closePort()
+        hex_log(HEX_LOG_LEVEL["info"], "HexRobotGello closed")

hex_zmq_servers/robot/gello/robot_gello_cli.py

@@ -0,0 +1,29 @@
+#!/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
+
+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,
+}
+
+
+class HexRobotGelloClient(HexRobotClientBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+    ):
+        HexRobotClientBase.__init__(self, net_config)
+        self._wait_for_working()

hex_zmq_servers/robot/gello/robot_gello_srv.py

@@ -0,0 +1,93 @@
+#!/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
+
+try:
+    from ..robot_base import HexRobotServerBase
+    from .robot_gello import HexRobotGello
+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.gello.robot_gello import HexRobotGello
+
+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 = {
+    "idxs": [0, 1, 2, 3, 4, 5, 6],
+    "invs": [1.0, -1.0, 1.0, 1.0, -1.0, 1.0, -4.0],
+    "limits": [[[-2.7, 2.7], [-1.57, 2.09], [0, 3.14], [-1.57, 1.57],
+                [-1.57, 1.57], [-1.57, 1.57], [0.0, 1.0]]],
+    "device":
+    "/dev/ttyUSB0",
+    "baudrate":
+    115200,
+    "max_retries":
+    3,
+    "torque_enabled":
+    False,
+    "sens_ts":
+    True,
+}
+
+
+class HexRobotGelloServer(HexRobotServerBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+        params_config: dict = ROBOT_CONFIG,
+    ):
+        HexRobotServerBase.__init__(self, net_config)
+
+        # robot
+        self._device = HexRobotGello(params_config,
+                                     net_config.get("realtime_mode", False))
+
+    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_cmds":
+            return self._set_cmds(recv_hdr, recv_buf)
+        else:
+            raise ValueError(f"unknown command: {recv_hdr['cmd']}")
+
+
+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, HexRobotGelloServer)

hex_zmq_servers/robot/hexarm/__init__.py

@@ -0,0 +1,47 @@
+#!/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_hexarm import HexRobotHexarm
+from .robot_hexarm_cli import HexRobotHexarmClient
+from .robot_hexarm_srv import HexRobotHexarmServer
+
+import os
+
+urdf_dir = os.path.join(os.path.dirname(__file__), "urdf")
+HEXARM_URDF_PATH_DICT = {
+    "archer_y6_empty":
+    f"{urdf_dir}/archer_y6/empty.urdf",
+    "archer_y6_gp100":
+    f"{urdf_dir}/archer_y6/gp100.urdf",
+    "archer_y6_gp100_handle":
+    f"{urdf_dir}/archer_y6/gp100_handle.urdf",
+    "archer_y6_gp100_p050":
+    f"{urdf_dir}/archer_y6/gp100_p050.urdf",
+    "archer_y6_gp100_p050_handle":
+    f"{urdf_dir}/archer_y6/gp100_p050_handle.urdf",
+    "archer_l6y_empty":
+    f"{urdf_dir}/archer_l6y/empty.urdf",
+    "archer_l6y_gp100":
+    f"{urdf_dir}/archer_l6y/gp100.urdf",
+    "archer_l6y_gp100_handle":
+    f"{urdf_dir}/archer_l6y/gp100_handle.urdf",
+    "archer_l6y_gp100_p050":
+    f"{urdf_dir}/archer_l6y/gp100_p050.urdf",
+    "archer_l6y_gp100_p050_handle":
+    f"{urdf_dir}/archer_l6y/gp100_p050_handle.urdf",
+}
+
+__all__ = [
+    # path
+    "HEXARM_URDF_PATH_DICT",
+
+    # robot
+    "HexRobotHexarm",
+    "HexRobotHexarmClient",
+    "HexRobotHexarmServer",
+]