hex-zmq-servers 0.2.3__tar.gz → 0.2.5__tar.gz

{hex_zmq_servers-0.2.3/hex_zmq_servers.egg-info → hex_zmq_servers-0.2.5}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: hex_zmq_servers
-Version: 0.2.3
+Version: 0.2.5
 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>
@@ -22,15 +22,19 @@ Requires-Python: >=3.8
 Description-Content-Type: text/markdown
 License-File: LICENSE
 Requires-Dist: pyzmq>=27.0.1
-Requires-Dist: opencv-python>=4.2
-Requires-Dist: mujoco>=3.3.3
-Requires-Dist: hex_device>=1.2.1
-Requires-Dist: hex_robo_utils>=0.1.18
+Requires-Dist: hex_device<1.4.0,>=1.3.1
+Requires-Dist: hex_robo_utils<0.2.0,>=0.1.19
 Requires-Dist: dynamixel-sdk==3.8.4
-Provides-Extra: berxel
-Requires-Dist: berxel_py_wrapper>=2.0; extra == "berxel"
+Provides-Extra: camera
+Requires-Dist: berxel_py_wrapper>=2.0; extra == "camera"
+Requires-Dist: opencv-python>=4.2; extra == "camera"
+Provides-Extra: mujoco
+Requires-Dist: mujoco>=3.3.3; extra == "mujoco"
+Requires-Dist: opencv-python>=4.2; extra == "mujoco"
 Provides-Extra: all
 Requires-Dist: berxel_py_wrapper>=2.0; extra == "all"
+Requires-Dist: mujoco>=3.3.3; extra == "all"
+Requires-Dist: opencv-python>=4.2; extra == "all"
 Dynamic: license-file
 
 # hex_zmq_servers
@@ -76,21 +80,45 @@ hex_zmq_servers/
 
 ### Install from PyPI
 
-For those who only want to use the library in their projects, it is recommended to install it from PyPI.
+1. For those who only want to use the library in their projects, it is recommended to install it from PyPI.
 
-```bash
-pip install hex_zmq_servers
-```
+    ```bash
+    pip install hex_zmq_servers[all]
+    ```
+
+2. If you don't want to install the extra dependencies for extra devices, you can run:
+
+    ```bash
+    pip install hex_zmq_servers
+    ```
 
 ### Install from Source Code
 
-For those who want to test the examples or contribute to the project, you can install it from source code.
+1. For those who want to test the examples or contribute to the project, you can install it from source code.
 
-```bash
-git clone https://github.com/hexfellow/hex_zmq_servers.git
-cd hex_zmq_servers
-./venv.sh --all
-```
+    ```bash
+    git clone https://github.com/hexfellow/hex_zmq_servers.git
+    cd hex_zmq_servers
+    ./venv.sh
+    ```
+
+2. If you don't want to install the extra dependencies for extra devices, you can run:
+
+    ```bash
+    git clone https://github.com/hexfellow/hex_zmq_servers.git
+    cd hex_zmq_servers
+    ./venv.sh --min
+    ```
+
+    (**Important**) Some examples would not work without the extra dependencies.
+
+3. If you don't want to install the examples, you can run:
+
+    ```bash
+    git clone https://github.com/hexfellow/hex_zmq_servers.git
+    cd hex_zmq_servers
+    ./venv.sh --pkg-only
+    ```
 
 ## Examples
 

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/README.md

@@ -41,21 +41,45 @@ hex_zmq_servers/
 
 ### Install from PyPI
 
-For those who only want to use the library in their projects, it is recommended to install it from PyPI.
+1. For those who only want to use the library in their projects, it is recommended to install it from PyPI.
 
-```bash
-pip install hex_zmq_servers
-```
+    ```bash
+    pip install hex_zmq_servers[all]
+    ```
+
+2. If you don't want to install the extra dependencies for extra devices, you can run:
+
+    ```bash
+    pip install hex_zmq_servers
+    ```
 
 ### Install from Source Code
 
-For those who want to test the examples or contribute to the project, you can install it from source code.
+1. For those who want to test the examples or contribute to the project, you can install it from source code.
 
-```bash
-git clone https://github.com/hexfellow/hex_zmq_servers.git
-cd hex_zmq_servers
-./venv.sh --all
-```
+    ```bash
+    git clone https://github.com/hexfellow/hex_zmq_servers.git
+    cd hex_zmq_servers
+    ./venv.sh
+    ```
+
+2. If you don't want to install the extra dependencies for extra devices, you can run:
+
+    ```bash
+    git clone https://github.com/hexfellow/hex_zmq_servers.git
+    cd hex_zmq_servers
+    ./venv.sh --min
+    ```
+
+    (**Important**) Some examples would not work without the extra dependencies.
+
+3. If you don't want to install the examples, you can run:
+
+    ```bash
+    git clone https://github.com/hexfellow/hex_zmq_servers.git
+    cd hex_zmq_servers
+    ./venv.sh --pkg-only
+    ```
 
 ## Examples
 

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/__init__.py

@@ -13,13 +13,6 @@ from .zmq_base import HexRate, hex_zmq_ts_now, hex_zmq_ts_delta_ms
 from .zmq_base import HexSafeValue, HexZMQClientBase, HexZMQServerBase, hex_server_helper
 from .zmq_base import HexZMQDummyClient, HexZMQDummyServer
 
-from .cam import HexCamBase, HexCamClientBase, HexCamServerBase
-from .cam import HexCamDummy, HexCamDummyClient, HexCamDummyServer
-
-from .mujoco import HexMujocoBase, HexMujocoClientBase, HexMujocoServerBase
-from .mujoco import HexMujocoArcherD6y, HexMujocoArcherD6yClient, HexMujocoArcherD6yServer
-from .mujoco import HexMujocoE3Desktop, HexMujocoE3DesktopClient, HexMujocoE3DesktopServer
-
 from .robot import HexRobotBase, HexRobotClientBase, HexRobotServerBase
 from .robot import HexRobotDummy, HexRobotDummyClient, HexRobotDummyServer
 from .robot import HexRobotGello, HexRobotGelloClient, HexRobotGelloServer
@@ -30,20 +23,12 @@ import os
 file_dir = os.path.dirname(os.path.abspath(__file__))
 HEX_ZMQ_SERVERS_PATH_DICT = {
     "zmq_dummy": f"{file_dir}/zmq_base.py",
-    "cam_dummy": f"{file_dir}/cam/dummy/cam_dummy_srv.py",
-    "mujoco_archer_d6y":
-    f"{file_dir}/mujoco/archer_d6y/mujoco_archer_d6y_srv.py",
-    "mujoco_e3_desktop":
-    f"{file_dir}/mujoco/e3_desktop/mujoco_e3_desktop_srv.py",
     "robot_dummy": f"{file_dir}/robot/dummy/robot_dummy_srv.py",
     "robot_gello": f"{file_dir}/robot/gello/robot_gello_srv.py",
     "robot_hexarm": f"{file_dir}/robot/hexarm/robot_hexarm_srv.py",
 }
 HEX_ZMQ_CONFIGS_PATH_DICT = {
     "zmq_dummy": f"{file_dir}/config/zmq_dummy.json",
-    "cam_dummy": f"{file_dir}/config/cam_dummy.json",
-    "mujoco_archer_d6y": f"{file_dir}/config/mujoco_archer_d6y.json",
-    "mujoco_e3_desktop": f"{file_dir}/config/mujoco_e3_desktop.json",
     "robot_dummy": f"{file_dir}/config/robot_dummy.json",
     "robot_gello": f"{file_dir}/config/robot_gello.json",
     "robot_hexarm": f"{file_dir}/config/robot_hexarm.json",
@@ -76,25 +61,6 @@ __all__ = [
     "HexZMQDummyClient",
     "HexZMQDummyServer",
 
-    # camera
-    "HexCamBase",
-    "HexCamClientBase",
-    "HexCamServerBase",
-    "HexCamDummy",
-    "HexCamDummyClient",
-    "HexCamDummyServer",
-
-    # mujoco
-    "HexMujocoBase",
-    "HexMujocoClientBase",
-    "HexMujocoServerBase",
-    "HexMujocoArcherD6y",
-    "HexMujocoArcherD6yClient",
-    "HexMujocoArcherD6yServer",
-    "HexMujocoE3Desktop",
-    "HexMujocoE3DesktopClient",
-    "HexMujocoE3DesktopServer",
-
     # robot
     "HexRobotBase",
     "HexRobotClientBase",
@@ -110,21 +76,72 @@ __all__ = [
     "HexRobotHexarmServer",
 ]
 
-# Optional: berxel
+# Optional: camera
 try:
+    from .cam import HexCamBase, HexCamClientBase, HexCamServerBase
+    from .cam import HexCamDummy, HexCamDummyClient, HexCamDummyServer
     from .cam import HexCamBerxel, HexCamBerxelClient, HexCamBerxelServer
-    HEX_ZMQ_SERVERS_PATH_DICT[
-        "cam_berxel"] = f"{file_dir}/cam/berxel/cam_berxel_srv.py"
-    HEX_ZMQ_CONFIGS_PATH_DICT[
-        "cam_berxel"] = f"{file_dir}/config/cam_berxel.json"
+    HEX_ZMQ_SERVERS_PATH_DICT.update({
+        "cam_dummy":
+        f"{file_dir}/cam/dummy/cam_dummy_srv.py",
+        "cam_berxel":
+        f"{file_dir}/cam/berxel/cam_berxel_srv.py",
+    })
+    HEX_ZMQ_CONFIGS_PATH_DICT.update({
+        "cam_dummy":
+        f"{file_dir}/config/cam_dummy.json",
+        "cam_berxel":
+        f"{file_dir}/config/cam_berxel.json",
+    })
     __all__.extend([
+        # camera
+        "HexCamBase",
+        "HexCamClientBase",
+        "HexCamServerBase",
+        "HexCamDummy",
+        "HexCamDummyClient",
+        "HexCamDummyServer",
         "HexCamBerxel",
         "HexCamBerxelClient",
         "HexCamBerxelServer",
     ])
 except ImportError:
     # berxel_py_wrapper not installed
-    # Install with: pip install hex_zmq_servers[berxel]
+    # Install with: pip install hex_zmq_servers[berxel] or pip install hex_zmq_servers[all]
+    pass
+
+# Optional: mujoco
+try:
+    from .mujoco import HexMujocoBase, HexMujocoClientBase, HexMujocoServerBase
+    from .mujoco import HexMujocoArcherD6y, HexMujocoArcherD6yClient, HexMujocoArcherD6yServer
+    from .mujoco import HexMujocoE3Desktop, HexMujocoE3DesktopClient, HexMujocoE3DesktopServer
+    HEX_ZMQ_SERVERS_PATH_DICT.update({
+        "mujoco_archer_d6y":
+        f"{file_dir}/mujoco/archer_d6y/mujoco_archer_d6y_srv.py",
+        "mujoco_e3_desktop":
+        f"{file_dir}/mujoco/e3_desktop/mujoco_e3_desktop_srv.py",
+    })
+    HEX_ZMQ_CONFIGS_PATH_DICT.update({
+        "mujoco_archer_d6y":
+        f"{file_dir}/config/mujoco_archer_d6y.json",
+        "mujoco_e3_desktop":
+        f"{file_dir}/config/mujoco_e3_desktop.json",
+    })
+    __all__.extend([
+        # mujoco
+        "HexMujocoBase",
+        "HexMujocoClientBase",
+        "HexMujocoServerBase",
+        "HexMujocoArcherD6y",
+        "HexMujocoArcherD6yClient",
+        "HexMujocoArcherD6yServer",
+        "HexMujocoE3Desktop",
+        "HexMujocoE3DesktopClient",
+        "HexMujocoE3DesktopServer",
+    ])
+except ImportError:
+    # mujoco not installed
+    # Install with: pip install hex_zmq_servers[mujoco] or pip install hex_zmq_servers[all]
     pass
 
 # print("#### Thanks for using hex_zmq_servers :D ####")

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/cam/__init__.py

@@ -8,6 +8,7 @@
 
 from .cam_base import HexCamBase, HexCamClientBase, HexCamServerBase
 from .dummy import HexCamDummy, HexCamDummyClient, HexCamDummyServer
+from .berxel import HexCamBerxel, HexCamBerxelClient, HexCamBerxelServer
 
 __all__ = [
     # base
@@ -19,17 +20,9 @@ __all__ = [
     "HexCamDummy",
     "HexCamDummyClient",
     "HexCamDummyServer",
-]
 
-# Optional: berxel (requires berxel_py_wrapper)
-try:
-    from .berxel import HexCamBerxel, HexCamBerxelClient, HexCamBerxelServer
-    __all__.extend([
-        "HexCamBerxel",
-        "HexCamBerxelClient",
-        "HexCamBerxelServer",
-    ])
-except ImportError:
-    # berxel_py_wrapper not installed
-    # Install with: pip install hex_zmq_servers[berxel]
-    pass
+    # berxel
+    "HexCamBerxel",
+    "HexCamBerxelClient",
+    "HexCamBerxelServer",
+]

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/cam/berxel/cam_berxel.py

@@ -7,10 +7,15 @@
 ################################################################
 
 import cv2
+import threading
 import numpy as np
 
 from ..cam_base import HexCamBase
-from ...zmq_base import HexSafeValue
+from ...zmq_base import (
+    hex_zmq_ts_now,
+    HexSafeValue,
+)
+from ...hex_launch import hex_log, HEX_LOG_LEVEL
 from berxel_py_wrapper import *
 
 CAMERA_CONFIG = {
@@ -69,13 +74,14 @@ class HexCamBerxel(HexCamBase):
         self._wait_for_working()
         return self.__serial_number
 
-    def work_loop(self, hex_values: list[HexSafeValue]):
+    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
         rgb_value = hex_values[0]
         depth_value = hex_values[1]
+        stop_event = hex_values[2]
 
         rgb_count = 0
         depth_count = 0
-        while self._working.is_set():
+        while self._working.is_set() and not stop_event.is_set():
             # read frame
             hawk_rgb_frame = self.__device.readColorFrame(40)
             hawk_depth_frame = self.__device.readDepthFrame(40)
@@ -95,6 +101,9 @@ class HexCamBerxel(HexCamBase):
             self.__device.releaseFrame(hawk_rgb_frame)
             self.__device.releaseFrame(hawk_depth_frame)
 
+        # close
+        self.close()
+
     def __unpack_frame(self, hawk_frame: BerxelHawkFrame, depth: bool = False):
         # common variables
         berxel_ts = hawk_frame.getTimeStamp()
@@ -132,7 +141,7 @@ class HexCamBerxel(HexCamBase):
             )
             frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
 
-        return ts, frame
+        return ts if self.__sens_ts else hex_zmq_ts_now(), frame
 
     def __open_device(self, serial_number: str | None = None) -> bool:
         # init context
@@ -220,9 +229,12 @@ class HexCamBerxel(HexCamBase):
             return False
 
     def close(self):
+        if not self._working.is_set():
+            return
         self._working.clear()
         self.__stop_stream()
         self.__close_device()
+        hex_log(HEX_LOG_LEVEL["info"], "HexCamBerxel closed")
 
     def __stop_stream(self):
         if self.__device is None:

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/cam/cam_base.py

@@ -6,6 +6,7 @@
 # Date  : 2025-09-16
 ################################################################
 
+import threading
 import numpy as np
 from abc import abstractmethod
 
@@ -30,7 +31,7 @@ class HexCamBase(HexDeviceBase):
         HexDeviceBase.__del__(self)
 
     @abstractmethod
-    def work_loop(self, hex_values: list[HexSafeValue]):
+    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
         raise NotImplementedError(
             "`work_loop` should be implemented by the child class")
 
@@ -97,7 +98,11 @@ class HexCamServerBase(HexZMQServerBase):
 
     def work_loop(self):
         try:
-            self._device.work_loop([self._rgb_value, self._depth_value])
+            self._device.work_loop([
+                self._rgb_value,
+                self._depth_value,
+                self._stop_event,
+            ])
         finally:
             self._device.close()
 

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/cam/dummy/cam_dummy.py

@@ -6,6 +6,7 @@
 # Date  : 2025-09-12
 ################################################################
 
+import threading
 import numpy as np
 
 from ..cam_base import HexCamBase
@@ -14,6 +15,7 @@ from ...zmq_base import (
     HexRate,
     HexSafeValue,
 )
+from ...hex_launch import hex_log, HEX_LOG_LEVEL
 
 
 class HexCamDummy(HexCamBase):
@@ -25,14 +27,15 @@ class HexCamDummy(HexCamBase):
     def __del__(self):
         HexCamBase.__del__(self)
 
-    def work_loop(self, hex_values: list[HexSafeValue]):
+    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
         rgb_value = hex_values[0]
         depth_value = hex_values[1]
+        stop_event = hex_values[2]
 
         rgb_count = 0
         depth_count = 0
         rate = HexRate(60)
-        while self._working.is_set():
+        while self._working.is_set() and not stop_event.is_set():
             # rgb
             rgb_img = np.random.randint(
                 0,
@@ -56,5 +59,11 @@ class HexCamDummy(HexCamBase):
             # sleep
             rate.sleep()
 
+        # close
+        self.close()
+
     def close(self):
+        if not self._working.is_set():
+            return
         self._working.clear()
+        hex_log(HEX_LOG_LEVEL["info"], "HexCamDummy closed")

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/config/mujoco_archer_d6y.json

@@ -7,7 +7,7 @@
         "server_num_workers": 4
     },
     "params": {
-        "states_rate": 250,
+        "states_rate": 500,
         "img_rate": 30,
         "tau_ctrl": false,
         "mit_kp": [

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/config/mujoco_e3_desktop.json

@@ -7,7 +7,7 @@
         "server_num_workers": 4
     },
     "params": {
-        "states_rate": 250,
+        "states_rate": 500,
         "img_rate": 30,
         "tau_ctrl": false,
         "mit_kp": [

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/config/robot_hexarm.json

@@ -9,7 +9,7 @@
     "params": {
         "device_ip": "172.18.8.161",
         "device_port": 8439,
-        "control_hz": 250,
+        "control_hz": 500,
         "arm_type": 16,
         "use_gripper": true,
         "mit_kp": [

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/device_base.py

@@ -32,7 +32,7 @@ class HexDeviceBase(ABC):
             self._working.wait(0.1)
 
     @abstractmethod
-    def work_loop(self, hex_values: list[HexSafeValue]):
+    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
         raise NotImplementedError(
             "`work_loop` should be implemented by the child class")
 

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/mujoco/archer_d6y/model/robot.xml

@@ -41,6 +41,13 @@
                                 <geom pos="0.04 0 0.12" quat="0.6830127 0.6830127 0.1830127 -0.1830127" type="mesh" rgba="0.1 0.1 0.1 1" mesh="camera_link" />
                                 <camera name="end_camera" pos="0.066 0 0.105" quat="0.8660254 0.0 -0.5 0.0" fovy="50"/>
 
+                                # ee frame
+                                <body name="ee_frame" pos="0.1870 0.0 0.0" quat="1 0 0 0">
+                                    <geom type="cylinder" size="0.002 0.01" pos="0.01 0.00 0.00" quat="0.7071068 0.0000000 0.7071068 0.0000000" rgba="1 0 0 1" contype="0" conaffinity="0" group="4"/>
+                                    <geom type="cylinder" size="0.002 0.01" pos="0.00 0.01 0.00" quat="0.7071068 0.7071068 0.0000000 0.0000000" rgba="0 1 0 1" contype="0" conaffinity="0" group="4"/>
+                                    <geom type="cylinder" size="0.002 0.01" pos="0.00 0.00 0.01" quat="1.0000000 0.0000000 0.0000000 0.0000000" rgba="0 0 1 1" contype="0" conaffinity="0" group="4"/>
+                                </body>
+
                                 # gripper helper
                                 <body name="gripper_left_helper_link" pos="0.057667 0.030604 2.5e-05">
                                     <inertial pos="0.00299357 0.0232625 -5e-05" quat="0.996724 0 0 0.0808839" mass="0.00594562" diaginertia="2.74798e-06 1.47567e-06 1.34732e-06"/>

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/mujoco/archer_d6y/model/scene.xml

@@ -1,7 +1,7 @@
 <mujoco model="archer_d6y scene">
     <compiler angle="radian"/>
     <option integrator="implicitfast"/>
-    <option timestep="4e-3"/>
+    <option timestep="2e-3"/>
 
     <statistic center="0.3 0 0.45" extent="0.8" meansize="0.05"/>
 

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/mujoco/archer_d6y/mujoco_archer_d6y.py

@@ -8,6 +8,7 @@
 
 import os
 import copy
+import threading
 import cv2
 import numpy as np
 
@@ -21,10 +22,11 @@ from ...zmq_base import (
     HexRate,
     HexSafeValue,
 )
+from ...hex_launch import hex_log, HEX_LOG_LEVEL
 from hex_robo_utils import HexCtrlUtilMitJoint as CtrlUtil
 
 MUJOCO_CONFIG = {
-    "states_rate": 250,
+    "states_rate": 500,
     "img_rate": 30,
     "tau_ctrl": False,
     "mit_kp": [200.0, 200.0, 200.0, 75.0, 15.0, 15.0, 20.0],
@@ -122,12 +124,13 @@ class HexMujocoArcherD6y(HexMujocoBase):
             self.__viewer.sync()
         return True
 
-    def work_loop(self, hex_values: list[HexSafeValue]):
+    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
         states_robot_value = hex_values[0]
         states_obj_value = hex_values[1]
         cmds_robot_value = hex_values[2]
         rgb_value = hex_values[3]
         depth_value = hex_values[4]
+        stop_event = hex_values[5]
 
         last_states_ts = {"s": 0, "ns": 0}
         states_robot_count = 0
@@ -140,7 +143,7 @@ class HexMujocoArcherD6y(HexMujocoBase):
         states_trig_count = 0
         img_trig_count = 0
 
-        while self._working.is_set():
+        while self._working.is_set() and not stop_event.is_set():
             states_trig_count += 1
             if states_trig_count >= self.__states_trig_thresh:
                 states_trig_count = 0
@@ -167,8 +170,7 @@ class HexMujocoArcherD6y(HexMujocoBase):
                 cmds_robot_pack = cmds_robot_value.get(timeout_s=-1.0)
                 if cmds_robot_pack is not None:
                     ts, seq, cmds = cmds_robot_pack
-                    delta_seq = (seq - last_cmds_robot_seq) % self._max_seq_num
-                    if delta_seq > 0 and delta_seq < 1e6:
+                    if seq != last_cmds_robot_seq:
                         last_cmds_robot_seq = seq
                         if hex_zmq_ts_delta_ms(hex_zmq_ts_now(), ts) < 200.0:
                             self.__set_cmds(cmds)
@@ -197,6 +199,9 @@ class HexMujocoArcherD6y(HexMujocoBase):
             # sleep
             rate.sleep()
 
+        # close
+        self.close()
+
     def __get_states(self):
         pos = copy.deepcopy(self.__data.qpos)
         vel = copy.deepcopy(self.__data.qvel)
@@ -213,23 +218,38 @@ class HexMujocoArcherD6y(HexMujocoBase):
     def __set_cmds(self, cmds: np.ndarray):
         tau_cmds = None
         if not self.__tau_ctrl:
+            cmd_pos = None
+            tar_vel = np.zeros_like(cmds)
+            cmd_tor = np.zeros_like(cmds)
+            cmd_kp = self.__mit_kp.copy()
+            cmd_kd = self.__mit_kd.copy()
             if len(cmds.shape) == 1:
-                cmd_pos = cmds.copy()
-                cmd_tor = np.zeros_like(cmds)
+                cmd_pos = cmds
+            elif len(cmds.shape) == 2:
+                if cmds.shape[1] == 2:
+                    cmd_pos = cmds[:, 0].copy()
+                    cmd_tor = cmds[:, 1].copy()
+                elif cmds.shape[1] == 5:
+                    cmd_pos = cmds[:, 0].copy()
+                    tar_vel = cmds[:, 1].copy()
+                    cmd_tor = cmds[:, 2].copy()
+                    cmd_kp = cmds[:, 3].copy()
+                    cmd_kd = cmds[:, 4].copy()
+                else:
+                    raise ValueError(f"The shape of cmds is invalid: {cmds.shape}")
             else:
-                cmd_pos = cmds[:, 0].copy()
-                cmd_tor = cmds[:, 1].copy()
-            cmd_pos = self._apply_pos_limits(
+                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],
             )
-            cmd_pos[-1] /= self.__gripper_ratio
+            tar_pos[-1] /= self.__gripper_ratio
             tau_cmds = self.__mit_ctrl(
-                self.__mit_kp,
-                self.__mit_kd,
-                cmd_pos,
-                np.zeros(len(self.__state_robot_idx)),
+                cmd_kp,
+                cmd_kd,
+                tar_pos,
+                tar_vel,
                 self.__data.qpos[self.__state_robot_idx],
                 self.__data.qvel[self.__state_robot_idx],
                 cmd_tor,
@@ -259,8 +279,11 @@ class HexMujocoArcherD6y(HexMujocoBase):
         }
 
     def close(self):
+        if not self._working.is_set():
+            return
         self._working.clear()
         self.__rgb_cam.close()
         self.__depth_cam.close()
         if not self.__headless:
             self.__viewer.close()
+        hex_log(HEX_LOG_LEVEL["info"], "HexMujocoArcherD6y closed")

{hex_zmq_servers-0.2.3 → hex_zmq_servers-0.2.5}/hex_zmq_servers/mujoco/archer_d6y/mujoco_archer_d6y_srv.py

@@ -31,7 +31,7 @@ NET_CONFIG = {
 }
 
 MUJOCO_CONFIG = {
-    "states_rate": 250,
+    "states_rate": 500,
     "img_rate": 30,
     "headless": False,
     "sens_ts": True,
@@ -64,6 +64,7 @@ class HexMujocoArcherD6yServer(HexMujocoServerBase):
                 self._cmds_value,
                 self._rgb_value,
                 self._depth_value,
+                self._stop_event,
             ])
         finally:
             self._device.close()
@@ -104,6 +105,8 @@ class HexMujocoArcherD6yServer(HexMujocoServerBase):
     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"] == "reset":
             return self.no_ts_hdr(recv_hdr, self._device.reset()), None
         elif recv_hdr["cmd"] == "get_dofs":