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

hex_zmq_servers/robot/hexarm/robot_hexarm.py

@@ -0,0 +1,292 @@
+#!/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, MotorBase
+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",
+    "use_gripper": True,
+    "mit_kp": [200.0, 200.0, 200.0, 75.0, 15.0, 15.0, 20.0],
+    "mit_kd": [12.5, 12.5, 12.5, 6.0, 0.31, 0.31, 1.0],
+    "sens_ts": True,
+}
+
+HEX_DEVICE_TYPE_DICT = {
+    "archer_y6": 16,
+    "archer_l6y": 17,
+}
+
+
+class HexRobotHexarm(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 = HEX_DEVICE_TYPE_DICT[robot_config["arm_type"]]
+            use_gripper = robot_config["use_gripper"]
+            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}")
+
+        self.__mit_kp = robot_config.get(
+            "mit_kp",
+            [200.0, 200.0, 200.0, 75.0, 15.0, 15.0, 20.0],
+        )
+        self.__mit_kd = robot_config.get(
+            "mit_kd",
+            [12.5, 12.5, 12.5, 6.0, 0.31, 0.31, 1.0],
+        )
+
+        # variables
+        # hex_arm variables
+        self.__hex_api: HexDeviceApi | None = None
+        self.__arm_archer: MotorBase | None = None
+        self.__gripper: MotorBase | 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_archer = self.__hex_api.find_device_by_robot_type(arm_type)
+        self.__arm_archer.start()
+        self.__arm_dofs = len(self.__arm_archer)
+        self._limits = self.__arm_archer.get_joint_limits()
+
+        # try to open gripper
+        self.__gripper_dofs = 0
+        self.__gripper = None
+        if use_gripper:
+            self.__gripper = self.__hex_api.find_optional_device_by_id(1)
+            if self.__gripper is not None:
+                self.__gripper_dofs = len(self.__gripper)
+                self._limits += [self.__gripper.get_joint_limits()]
+            else:
+                print("\033[33mGripper not found\033[0m")
+
+        # modify variables
+        self._dofs = [self.__arm_dofs + self.__gripper_dofs]
+        self._limits = np.ascontiguousarray(np.asarray(self._limits)).reshape(
+            self._dofs[0], 3, 2)
+        self.__mit_kp = np.ascontiguousarray(np.asarray(self.__mit_kp))
+        self.__mit_kd = np.ascontiguousarray(np.asarray(self.__mit_kd))
+        if self.__mit_kp.shape[0] < self._dofs[0] or self.__mit_kd.shape[
+                0] < self._dofs[0]:
+            raise ValueError(
+                "The length of mit_kp and mit_kd must be greater than or equal to the number of motors"
+            )
+        elif self.__mit_kp.shape[0] > self._dofs[0] or self.__mit_kd.shape[
+                0] > self._dofs[0]:
+            print(
+                f"\033[33mThe length of mit_kp and mit_kd is greater than the number of motors\033[0m"
+            )
+            self.__mit_kp = self.__mit_kp[:self._dofs[0]]
+            self.__mit_kd = self.__mit_kd[:self._dofs[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]
+
+        last_states_ts = hex_zmq_ts_now()
+        states_count = 0
+        last_cmds_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_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) -> tuple[np.ndarray | None, dict | None]:
+        if self.__arm_archer is None:
+            return None, None
+
+        # (arm_dofs, 3) # pos vel eff
+        if self.__arm_state_buffer is None:
+            self.__arm_state_buffer = self.__arm_archer.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_archer is None:
+            print("\033[91mArm not found\033[0m")
+            return False
+
+        if cmds.shape[0] < self._dofs[0]:
+            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[0]:
+            print(
+                f"\033[33mThe length of joint_angles is greater than the number of motors\033[0m"
+            )
+            cmds = cmds[:self._dofs[0]]
+
+        cmd_pos = None
+        tar_vel = np.zeros(self._dofs[0])
+        cmd_tor = np.zeros(self._dofs[0])
+        cmd_kp = self.__mit_kp.copy()
+        cmd_kd = self.__mit_kd.copy()
+        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
+        mit_cmd = self.__arm_archer.construct_mit_command(
+            tar_pos[:self.__arm_dofs],
+            tar_vel[:self.__arm_dofs],
+            cmd_tor[:self.__arm_dofs],
+            cmd_kp[:self.__arm_dofs],
+            cmd_kd[:self.__arm_dofs],
+        )
+        self.__arm_archer.motor_command(CommandType.MIT, mit_cmd)
+
+        # gripper
+        if self.__gripper is not None:
+            mit_cmd = self.__gripper.construct_mit_command(
+                tar_pos[self.__arm_dofs:],
+                tar_vel[self.__arm_dofs:],
+                cmd_tor[self.__arm_dofs:],
+                cmd_kp[self.__arm_dofs:],
+                cmd_kd[self.__arm_dofs:],
+            )
+            self.__gripper.motor_command(CommandType.MIT, mit_cmd)
+
+        return True
+
+    def close(self):
+        if not self._working.is_set():
+            return
+        self._working.clear()
+        self.__arm_archer.stop()
+        self.__hex_api.close()
+        hex_log(HEX_LOG_LEVEL["info"], "HexRobotHexarm closed")

hex_zmq_servers/robot/hexarm/robot_hexarm_cli.py

@@ -0,0 +1,37 @@
+#!/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,
+}
+
+ROBOT_CONFIG = {
+    "device_ip": "172.18.8.161",
+    "device_port": 8439,
+    "control_hz": 250,
+    "arm_type": "archer_l6y",
+    "sens_ts": True,
+}
+
+
+class HexRobotHexarmClient(HexRobotClientBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+    ):
+        HexRobotClientBase.__init__(self, net_config)
+        self._wait_for_working()

hex_zmq_servers/robot/hexarm/robot_hexarm_srv.py

@@ -0,0 +1,87 @@
+#!/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_hexarm import HexRobotHexarm
+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.hexarm.robot_hexarm import HexRobotHexarm
+
+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,
+    "arm_type": "archer_l6y",
+    "use_gripper": True,
+    "mit_kp": [200.0, 200.0, 200.0, 75.0, 15.0, 15.0, 0.0],
+    "mit_kd": [12.5, 12.5, 12.5, 6.0, 0.31, 0.31, 0.0],
+    "sens_ts": True,
+}
+
+
+class HexRobotHexarmServer(HexRobotServerBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+        params_config: dict = ROBOT_CONFIG,
+    ):
+        HexRobotServerBase.__init__(self, net_config)
+
+        # robot
+        self._device = HexRobotHexarm(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, HexRobotHexarmServer)

hex_zmq_servers/robot/hexarm/urdf/archer_l6y/empty.urdf

@@ -0,0 +1,206 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="archer_l6y">
+    <link name="base_link">
+        <inertial>
+            <origin xyz="-0.00080975 0.00005863 0.05684460" rpy="0 0 0" />
+            <mass value="0.61800543" />
+            <inertia ixx="0.00055618" ixy="-0.00000009" ixz="-0.00000663" iyy="0.00058624" iyz="0.00000086" izz="0.00060508" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/base_link.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/base_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+
+    <link name="link_1">
+        <inertial>
+            <origin xyz="0.00884023 -0.00039740 0.03457242" rpy="0 0 0" />
+            <mass value="0.29593000" />
+            <inertia ixx="0.00047871" ixy="0.00000088" ixz="-0.00005799" iyy="0.00030053" iyz="0.00000258" izz="0.00041726" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_1.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_1.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_1" type="revolute">
+        <origin xyz="0 0 0.08355" rpy="0 0 0" />
+        <parent link="base_link" />
+        <child link="link_1" />
+        <axis xyz="0 0 1" />
+        <limit lower="-2.8623" upper="2.8623" effort="1" velocity="1" />
+    </joint>
+
+    <link name="link_2">
+        <inertial>
+            <origin xyz="-0.00159885 0.02740310 0.13379837" rpy="0 0 0" />
+            <mass value="1.34359000" />
+            <inertia ixx="0.01905633" ixy="0.00000078" ixz="0.00001635" iyy="0.01890424" iyz="0.00000765" izz="0.00086433" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_2.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_2.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_2" type="revolute">
+        <origin xyz="0.02 -0.0285 0.0555" rpy="0 0 0" />
+        <parent link="link_1" />
+        <child link="link_2" />
+        <axis xyz="0 1 0" />
+        <limit lower="-2.0943" upper="1.5707" effort="1" velocity="1" />
+    </joint>
+
+    <link name="link_3">
+        <inertial>
+            <origin xyz="-0.05454198 0.02770576 0.14631127" rpy="0 0 0" />
+            <mass value="1.08626000" />
+            <inertia ixx="0.01172853" ixy="-0.00000662" ixz="0.00163495" iyy="0.01182587" iyz="0.00007450" izz="0.00147592" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_3.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_3.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_3" type="revolute">
+        <origin xyz="0 0 0.27" rpy="0 0 0" />
+        <parent link="link_2" />
+        <child link="link_3" />
+        <axis xyz="0 1 0" />
+        <limit lower="0" upper="3.159" effort="1" velocity="1" />
+    </joint>
+
+    <link name="link_4">
+        <inertial>
+            <origin xyz="-0.04639218 0.02832840 0.04493954" rpy="0 0 0" />
+            <mass value="0.53458000" />
+            <inertia ixx="0.00085129" ixy="0.00000633" ixz="0.00043133" iyy="0.00109396" iyz="-0.00000547" izz="0.00087351" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_4.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_4.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_4" type="revolute">
+        <origin xyz="-0.065 -5E-05 0.25" rpy="0 0 0" />
+        <parent link="link_3" />
+        <child link="link_4" />
+        <axis xyz="0 1 0" />
+        <limit lower="-1.5707" upper="1.5707" effort="1" velocity="1" />
+    </joint>
+
+    <link name="link_5">
+        <inertial>
+            <origin xyz="0.04082486 0.00006149 0.00717093" rpy="0 0 0" />
+            <mass value="0.38996000" />
+            <inertia ixx="0.00016500" ixy="-0.00000021" ixz="-0.00002287" iyy="0.00022995" iyz="0.00000039" izz="0.00021724" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_5.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_5.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_5" type="revolute">
+        <origin xyz="-0.046 0.0285 0.065" rpy="0 0 0" />
+        <parent link="link_4" />
+        <child link="link_5" />
+        <axis xyz="-1 0 0" />
+        <limit lower="-1.5707" upper="1.5707" effort="1" velocity="1" />
+    </joint>
+
+    <link name="link_6">
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <mass value="0.001" />
+            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_6.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_l6y/meshes/link_6.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_6" type="revolute">
+        <origin xyz="0.046 0 0.034" rpy="0 0 0" />
+        <parent link="link_5" />
+        <child link="link_6" />
+        <axis xyz="0 0 1" />
+        <limit lower="-3.1415" upper="3.1415" effort="0" velocity="0" />
+    </joint>
+
+</robot>