hex-zmq-servers 0.3.3__py3-none-any.whl → 0.3.4__py3-none-any.whl

hex_zmq_servers/mujoco/e3_desktop/mujoco_e3_desktop_srv.py

@@ -7,7 +7,7 @@
 ################################################################
 
 import numpy as np
-from hex_zmq_servers.zmq_base import HexSafeValue
+from collections import deque
 
 try:
     from ..mujoco_base import HexMujocoServerBase
@@ -31,7 +31,7 @@ NET_CONFIG = {
 }
 
 MUJOCO_CONFIG = {
-    "states_rate": 500,
+    "states_rate": 1000,
     "img_rate": 30,
     "headless": False,
     "sens_ts": True,
@@ -53,32 +53,32 @@ class HexMujocoE3DesktopServer(HexMujocoServerBase):
         # values
         self._cmds_left_seq = -1
         self._cmds_right_seq = -1
-        self._states_left_value = HexSafeValue()
-        self._states_right_value = HexSafeValue()
-        self._states_obj_value = HexSafeValue()
-        self._cmds_left_value = HexSafeValue()
-        self._cmds_right_value = HexSafeValue()
-        self._rgb_head_value = HexSafeValue()
-        self._depth_head_value = HexSafeValue()
-        self._rgb_left_value = HexSafeValue()
-        self._depth_left_value = HexSafeValue()
-        self._rgb_right_value = HexSafeValue()
-        self._depth_right_value = HexSafeValue()
+        self._states_left_queue = deque(maxlen=10)
+        self._states_right_queue = deque(maxlen=10)
+        self._states_obj_queue = deque(maxlen=10)
+        self._cmds_left_queue = deque(maxlen=10)
+        self._cmds_right_queue = deque(maxlen=10)
+        self._rgb_head_queue = deque(maxlen=10)
+        self._depth_head_queue = deque(maxlen=10)
+        self._rgb_left_queue = deque(maxlen=10)
+        self._depth_left_queue = deque(maxlen=10)
+        self._rgb_right_queue = deque(maxlen=10)
+        self._depth_right_queue = deque(maxlen=10)
 
     def work_loop(self):
         try:
             self._device.work_loop([
-                self._states_left_value,
-                self._states_right_value,
-                self._states_obj_value,
-                self._cmds_left_value,
-                self._cmds_right_value,
-                self._rgb_head_value,
-                self._depth_head_value,
-                self._rgb_left_value,
-                self._depth_left_value,
-                self._rgb_right_value,
-                self._depth_right_value,
+                self._states_left_queue,
+                self._states_right_queue,
+                self._states_obj_queue,
+                self._cmds_left_queue,
+                self._cmds_right_queue,
+                self._rgb_head_queue,
+                self._depth_head_queue,
+                self._rgb_left_queue,
+                self._depth_left_queue,
+                self._rgb_right_queue,
+                self._depth_right_queue,
                 self._stop_event,
             ])
         finally:
@@ -94,16 +94,18 @@ class HexMujocoE3DesktopServer(HexMujocoServerBase):
         # get robot name
         robot_name = recv_hdr["cmd"].split("_")[2]
         if robot_name == "left":
-            value = self._states_left_value
+            queue = self._states_left_queue
         elif robot_name == "right":
-            value = self._states_right_value
+            queue = self._states_right_queue
         elif robot_name == "obj":
-            value = self._states_obj_value
+            queue = self._states_obj_queue
         else:
             raise ValueError(f"unknown robot name: {robot_name}")
 
         try:
-            ts, count, states = value.get()
+            ts, count, states = queue.popleft()
+        except IndexError:
+            return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
         except Exception as e:
             print(f"\033[91m{recv_hdr['cmd']} failed: {e}\033[0m")
             return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
@@ -129,10 +131,10 @@ class HexMujocoE3DesktopServer(HexMujocoServerBase):
         # get robot name
         robot_name = recv_hdr["cmd"].split("_")[2]
         if robot_name == "left":
-            value = self._cmds_left_value
+            queue = self._cmds_left_queue
             cmds_seq = self._cmds_left_seq
         elif robot_name == "right":
-            value = self._cmds_right_value
+            queue = self._cmds_right_queue
             cmds_seq = self._cmds_right_seq
         else:
             raise ValueError(f"unknown robot name: {robot_name}")
@@ -144,7 +146,7 @@ class HexMujocoE3DesktopServer(HexMujocoServerBase):
                     self._cmds_left_seq = cmds_seq
                 elif robot_name == "right":
                     self._cmds_right_seq = cmds_seq
-                value.set((recv_hdr["ts"], seq, recv_buf))
+                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
@@ -163,16 +165,18 @@ class HexMujocoE3DesktopServer(HexMujocoServerBase):
         depth_flag = split_cmd[1] == "depth"
         camera_name = split_cmd[2]
         if camera_name == "head":
-            value = self._rgb_head_value if not depth_flag else self._depth_head_value
+            queue = self._rgb_head_queue if not depth_flag else self._depth_head_queue
         elif camera_name == "left":
-            value = self._rgb_left_value if not depth_flag else self._depth_left_value
+            queue = self._rgb_left_queue if not depth_flag else self._depth_left_queue
         elif camera_name == "right":
-            value = self._rgb_right_value if not depth_flag else self._depth_right_value
+            queue = self._rgb_right_queue if not depth_flag else self._depth_right_queue
         else:
             raise ValueError(f"unknown camera name: {camera_name}")
 
         try:
-            ts, count, img = value.get()
+            ts, count, img = queue.popleft()
+        except IndexError:
+            return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
         except Exception as e:
             print(f"\033[91m{recv_hdr['cmd']} failed: {e}\033[0m")
             return {"cmd": f"{recv_hdr['cmd']}_failed"}, None

hex_zmq_servers/mujoco/mujoco_base.py

@@ -8,12 +8,12 @@
 
 import threading
 import numpy as np
+from collections import deque
 from abc import abstractmethod
 
 from ..device_base import HexDeviceBase
 from ..zmq_base import (
     hex_zmq_ts_now,
-    HexSafeValue,
     HexZMQClientBase,
     HexZMQServerBase,
 )
@@ -88,7 +88,7 @@ class HexMujocoBase(HexDeviceBase):
         return normed_rads
 
     @abstractmethod
-    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
+    def work_loop(self, hex_queues: list[deque | threading.Event]):
         raise NotImplementedError(
             "`work_loop` should be implemented by the child class")
 
@@ -107,6 +107,13 @@ class HexMujocoClientBase(HexZMQClientBase):
         self._cmds_seq = 0
         self._rgb_seq = 0
         self._depth_seq = 0
+        self._states_queue = {
+            "robot": deque(maxlen=10),
+            "obj": deque(maxlen=10),
+        }
+        self._rgb_queue = deque(maxlen=10)
+        self._depth_queue = deque(maxlen=10)
+        self._cmds_queue = deque(maxlen=10)
 
     def __del__(self):
         HexZMQClientBase.__del__(self)
@@ -133,7 +140,77 @@ class HexMujocoClientBase(HexZMQClientBase):
         _, limits = self.request({"cmd": "get_limits"})
         return limits
 
-    def get_states(self, robot_name: str | None = None):
+    def get_states(self, robot_name: str | None = None, newest: bool = False):
+        try:
+            return self._states_queue[robot_name].popleft(
+            ) if not newest else self._states_queue[robot_name][-1]
+        except IndexError:
+            return None, None
+        except KeyError:
+            print(f"\033[91munknown robot name: {robot_name}\033[0m")
+            return None, None
+
+    def get_rgb(self, camera_name: str | None = None, newest: bool = False):
+        try:
+            return self._rgb_queue.popleft(
+            ) if not newest else self._rgb_queue[-1]
+        except IndexError:
+            return None, None
+
+    def get_depth(self, camera_name: str | None = None, newest: bool = False):
+        try:
+            return self._depth_queue.popleft(
+            ) if not newest else self._depth_queue[-1]
+        except IndexError:
+            return None, None
+
+    def set_cmds(self, cmds: np.ndarray):
+        self._cmds_queue.append(cmds)
+
+    def get_intri(self):
+        intri_hdr, intri = self.request({"cmd": "get_intri"})
+        return intri_hdr, intri
+
+    def _get_rgb_inner(self, camera_name: str | None = None):
+        return self._process_frame(camera_name, False)
+
+    def _get_depth_inner(self, camera_name: str | None = None):
+        return self._process_frame(camera_name, True)
+
+    def _process_frame(
+        self,
+        camera_name: str | None = None,
+        depth_flag: bool = False,
+    ):
+        req_cmd = f"get_{'depth' if depth_flag else 'rgb'}"
+        if camera_name is not None:
+            req_cmd += f"_{camera_name}"
+
+        hdr, img = self.request({
+            "cmd":
+            req_cmd,
+            "args": (1 + (self._depth_seq if depth_flag else self._rgb_seq)) %
+            self._max_seq_num,
+        })
+
+        try:
+            cmd = hdr["cmd"]
+            if cmd == f"{req_cmd}_ok":
+                if depth_flag:
+                    self._depth_seq = hdr["args"]
+                else:
+                    self._rgb_seq = hdr["args"]
+                return hdr, img
+            else:
+                return None, None
+        except KeyError:
+            print(f"\033[91m{hdr['cmd']} requires `cmd`\033[0m")
+            return None, None
+        except Exception as e:
+            print(f"\033[91m__process_frame failed: {e}\033[0m")
+            return None, None
+
+    def _get_states_inner(self, robot_name: str | None = None):
         req_cmd = "get_states"
         if robot_name is not None:
             req_cmd += f"_{robot_name}"
@@ -156,7 +233,7 @@ class HexMujocoClientBase(HexZMQClientBase):
             print(f"\033[91m{req_cmd} failed: {e}\033[0m")
             return None, None
 
-    def set_cmds(
+    def _set_cmds_inner(
         self,
         cmds: np.ndarray,
     ) -> bool:
@@ -184,71 +261,28 @@ class HexMujocoClientBase(HexZMQClientBase):
             print(f"\033[91m{req_cmd} failed: {e}\033[0m")
             return False
 
-    def get_intri(self):
-        intri_hdr, intri = self.request({"cmd": "get_intri"})
-        return intri_hdr, intri
-
-    def get_rgb(self, camera_name: str | None = None):
-        return self._process_frame(camera_name, False)
-
-    def get_depth(self, camera_name: str | None = None):
-        return self._process_frame(camera_name, True)
-
-    def _process_frame(
-        self,
-        camera_name: str | None = None,
-        depth_flag: bool = False,
-    ):
-        req_cmd = f"get_{'depth' if depth_flag else 'rgb'}"
-        if camera_name is not None:
-            req_cmd += f"_{camera_name}"
-
-        hdr, img = self.request({
-            "cmd":
-            req_cmd,
-            "args": (1 + (self._depth_seq if depth_flag else self._rgb_seq)) %
-            self._max_seq_num,
-        })
-
-        try:
-            cmd = hdr["cmd"]
-            if cmd == f"{req_cmd}_ok":
-                if depth_flag:
-                    self._depth_seq = hdr["args"]
-                else:
-                    self._rgb_seq = hdr["args"]
-                return hdr, img
-            else:
-                return None, None
-        except KeyError:
-            print(f"\033[91m{hdr['cmd']} requires `cmd`\033[0m")
-            return None, None
-        except Exception as e:
-            print(f"\033[91m__process_frame failed: {e}\033[0m")
-            return None, None
-
 
 class HexMujocoServerBase(HexZMQServerBase):
 
     def __init__(self, net_config: dict = NET_CONFIG):
         HexZMQServerBase.__init__(self, net_config)
         self._device: HexDeviceBase = None
-        self._states_value = HexSafeValue()
-        self._obj_pose_value = HexSafeValue()
-        self._cmds_value = HexSafeValue()
+        self._states_queue = deque(maxlen=10)
+        self._obj_pose_queue = deque(maxlen=10)
+        self._cmds_queue = deque(maxlen=10)
         self._cmds_seq = -1
-        self._rgb_value = HexSafeValue()
-        self._depth_value = HexSafeValue()
+        self._rgb_queue = deque(maxlen=10)
+        self._depth_queue = deque(maxlen=10)
         self._seq_clear_flag = False
 
     def work_loop(self):
         try:
             self._device.work_loop([
-                self._states_value,
-                self._obj_pose_value,
-                self._cmds_value,
-                self._rgb_value,
-                self._depth_value,
+                self._states_queue,
+                self._obj_pose_queue,
+                self._cmds_queue,
+                self._rgb_queue,
+                self._depth_queue,
                 self._stop_event,
             ])
         finally:
@@ -266,7 +300,9 @@ class HexMujocoServerBase(HexZMQServerBase):
             return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
 
         try:
-            ts, count, states = self._states_value.get()
+            ts, count, states = self._states_queue.popleft()
+        except IndexError:
+            return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
         except Exception as e:
             print(f"\033[91m{recv_hdr['cmd']} failed: {e}\033[0m")
             return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
@@ -292,7 +328,7 @@ class HexMujocoServerBase(HexZMQServerBase):
             delta = (seq - self._cmds_seq) % self._max_seq_num
             if delta >= 0 and delta < 1e6:
                 self._cmds_seq = seq
-                self._cmds_value.set((recv_hdr["ts"], seq, recv_buf))
+                self._cmds_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
@@ -309,13 +345,14 @@ class HexMujocoServerBase(HexZMQServerBase):
         # get camera config
         split_cmd = recv_hdr["cmd"].split("_")
         depth_flag = split_cmd[1] == "depth"
-        if depth_flag:
-            value = self._depth_value
-        else:
-            value = self._rgb_value
 
         try:
-            ts, count, img = value.get()
+            if depth_flag:
+                ts, count, img = self._depth_queue.popleft()
+            else:
+                ts, count, img = self._rgb_queue.popleft()
+        except IndexError:
+            return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
         except Exception as e:
             print(f"\033[91m{recv_hdr['cmd']} failed: {e}\033[0m")
             return {"cmd": f"{recv_hdr['cmd']}_failed"}, None

hex_zmq_servers/robot/dummy/robot_dummy.py

@@ -8,13 +8,13 @@
 
 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,
-    HexSafeValue,
 )
 from ...hex_launch import hex_log, HEX_LOG_LEVEL
 
@@ -47,10 +47,10 @@ class HexRobotDummy(HexRobotBase):
     def __del__(self):
         HexRobotBase.__del__(self)
 
-    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
-        states_value = hex_values[0]
-        cmds_value = hex_values[1]
-        stop_event = hex_values[2]
+    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]
 
         dummy_states = np.zeros((self._dofs[0], 3))
         states_count = 0
@@ -58,11 +58,15 @@ class HexRobotDummy(HexRobotBase):
         rate = HexRate(1000)
         while self._working.is_set() and not stop_event.is_set():
             # states
-            states_value.set((hex_zmq_ts_now(), states_count, dummy_states))
+            states_queue.append((hex_zmq_ts_now(), states_count, dummy_states))
             states_count = (states_count + 1) % self._max_seq_num
 
             # cmds
-            cmds_pack = cmds_value.get(timeout_s=-1.0)
+            cmds_pack = None
+            try:
+                cmds_pack = cmds_queue.popleft()
+            except IndexError:
+                pass
             if cmds_pack is not None:
                 ts, seq, cmds = cmds_pack
                 if seq != last_cmds_seq:

hex_zmq_servers/robot/dummy/robot_dummy_cli.py

@@ -24,3 +24,4 @@ class HexRobotDummyClient(HexRobotClientBase):
         net_config: dict = NET_CONFIG,
     ):
         HexRobotClientBase.__init__(self, net_config)
+        self._wait_for_working()

hex_zmq_servers/robot/gello/robot_gello.py

@@ -11,13 +11,13 @@ 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,
-    HexSafeValue,
 )
 from ...hex_launch import hex_log, HEX_LOG_LEVEL
 from dynamixel_sdk.group_sync_read import GroupSyncRead
@@ -104,10 +104,10 @@ class HexRobotGello(HexRobotBase):
         # start work loop
         self._working.set()
 
-    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
-        states_value = hex_values[0]
-        cmds_value = hex_values[1]
-        stop_event = hex_values[2]
+    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
@@ -116,11 +116,15 @@ class HexRobotGello(HexRobotBase):
             # states
             ts, states = self.__get_states()
             if states is not None:
-                states_value.set((ts, states_count, states))
+                states_queue.append((ts, states_count, states))
                 states_count = (states_count + 1) % self._max_seq_num
 
             # cmds
-            cmds_pack = cmds_value.get(timeout_s=-1.0)
+            cmds_pack = None
+            try:
+                cmds_pack = cmds_queue.popleft()
+            except IndexError:
+                pass
             if cmds_pack is not None:
                 ts, seq, cmds = cmds_pack
                 if seq != last_cmds_seq:

hex_zmq_servers/robot/gello/robot_gello_cli.py

@@ -24,3 +24,4 @@ class HexRobotGelloClient(HexRobotClientBase):
         net_config: dict = NET_CONFIG,
     ):
         HexRobotClientBase.__init__(self, net_config)
+        self._wait_for_working()

hex_zmq_servers/robot/hexarm/robot_hexarm.py

@@ -9,13 +9,13 @@
 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,
-    HexSafeValue,
 )
 from ...hex_launch import hex_log, HEX_LOG_LEVEL
 from hex_device import HexDeviceApi, MotorBase
@@ -73,6 +73,10 @@ class HexRobotHexarm(HexRobotBase):
         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}",
@@ -122,26 +126,30 @@ class HexRobotHexarm(HexRobotBase):
         # start work loop
         self._working.set()
 
-    def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
-        states_value = hex_values[0]
-        cmds_value = hex_values[1]
-        stop_event = hex_values[2]
+    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(1000)
+        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) > 1.0:
+                if hex_zmq_ts_delta_ms(ts, last_states_ts) > 1e-6:
                     last_states_ts = ts
-                    states_value.set((ts, states_count, states))
+                    states_queue.append((ts, states_count, states))
                     states_count = (states_count + 1) % self._max_seq_num
 
             # cmds
-            cmds_pack = cmds_value.get(timeout_s=-1.0)
+            cmds_pack = None
+            try:
+                cmds_pack = cmds_queue.popleft()
+            except IndexError:
+                pass
             if cmds_pack is not None:
                 ts, seq, cmds = cmds_pack
                 if seq != last_cmds_seq:
@@ -160,22 +168,47 @@ class HexRobotHexarm(HexRobotBase):
             return None, None
 
         # (arm_dofs, 3) # pos vel eff
-        arm_states_dict = self.__arm_archer.get_simple_motor_status()
-        pos = arm_states_dict['pos']
-        vel = arm_states_dict['vel']
-        eff = arm_states_dict['eff']
-        ts = arm_states_dict['ts']
+        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:
-            gripper_states_dict = self.__gripper.get_simple_motor_status()
-            pos += gripper_states_dict['pos']
-            vel += gripper_states_dict['vel']
-            eff += gripper_states_dict['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
 
-        pos, vel, eff = np.asarray(pos), np.asarray(vel), np.asarray(eff)
-        return ts if self.__sens_ts else hex_zmq_ts_now(), np.array(
-            [pos, vel, eff]).T
+        return None, None
 
     def __set_cmds(self, cmds: np.ndarray) -> bool:
         # cmds: (n)
@@ -254,5 +287,6 @@ class HexRobotHexarm(HexRobotBase):
             return
         self._working.clear()
         self.__arm_archer.stop()
+        time.sleep(0.5)
         self.__hex_api.close()
         hex_log(HEX_LOG_LEVEL["info"], "HexRobotHexarm closed")

hex_zmq_servers/robot/hexarm/robot_hexarm_cli.py

@@ -32,3 +32,4 @@ class HexRobotHexarmClient(HexRobotClientBase):
         net_config: dict = NET_CONFIG,
     ):
         HexRobotClientBase.__init__(self, net_config)
+        self._wait_for_working()