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

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Files changed (45) hide show
  1. hex_zmq_servers/__init__.py +45 -20
  2. hex_zmq_servers/cam/__init__.py +29 -5
  3. hex_zmq_servers/cam/berxel/cam_berxel.py +8 -10
  4. hex_zmq_servers/cam/berxel/cam_berxel_cli.py +1 -0
  5. hex_zmq_servers/cam/cam_base.py +40 -10
  6. hex_zmq_servers/cam/dummy/cam_dummy.py +7 -7
  7. hex_zmq_servers/cam/dummy/cam_dummy_cli.py +1 -0
  8. hex_zmq_servers/cam/realsense/__init__.py +17 -0
  9. hex_zmq_servers/cam/realsense/cam_realsense.py +157 -0
  10. hex_zmq_servers/cam/realsense/cam_realsense_cli.py +31 -0
  11. hex_zmq_servers/cam/realsense/cam_realsense_srv.py +75 -0
  12. hex_zmq_servers/cam/rgb/__init__.py +17 -0
  13. hex_zmq_servers/cam/rgb/cam_rgb.py +133 -0
  14. hex_zmq_servers/cam/rgb/cam_rgb_cli.py +41 -0
  15. hex_zmq_servers/cam/rgb/cam_rgb_srv.py +75 -0
  16. hex_zmq_servers/config/cam_realsense.json +15 -0
  17. hex_zmq_servers/config/cam_rgb.json +26 -0
  18. hex_zmq_servers/config/mujoco_archer_y6.json +2 -1
  19. hex_zmq_servers/config/mujoco_e3_desktop.json +6 -1
  20. hex_zmq_servers/config/robot_hexarm.json +1 -1
  21. hex_zmq_servers/device_base.py +3 -2
  22. hex_zmq_servers/mujoco/archer_y6/model/robot.xml +6 -6
  23. hex_zmq_servers/mujoco/archer_y6/model/scene.xml +1 -1
  24. hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6.py +74 -39
  25. hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6_cli.py +42 -0
  26. hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6_srv.py +16 -14
  27. hex_zmq_servers/mujoco/e3_desktop/model/robot.xml +12 -12
  28. hex_zmq_servers/mujoco/e3_desktop/model/scene.xml +1 -1
  29. hex_zmq_servers/mujoco/e3_desktop/mujoco_e3_desktop.py +138 -70
  30. hex_zmq_servers/mujoco/e3_desktop/mujoco_e3_desktop_cli.py +148 -33
  31. hex_zmq_servers/mujoco/e3_desktop/mujoco_e3_desktop_srv.py +39 -35
  32. hex_zmq_servers/mujoco/mujoco_base.py +101 -64
  33. hex_zmq_servers/robot/dummy/robot_dummy.py +11 -7
  34. hex_zmq_servers/robot/dummy/robot_dummy_cli.py +1 -0
  35. hex_zmq_servers/robot/gello/robot_gello.py +11 -7
  36. hex_zmq_servers/robot/gello/robot_gello_cli.py +1 -0
  37. hex_zmq_servers/robot/hexarm/robot_hexarm.py +56 -22
  38. hex_zmq_servers/robot/hexarm/robot_hexarm_cli.py +1 -0
  39. hex_zmq_servers/robot/robot_base.py +40 -10
  40. hex_zmq_servers/zmq_base.py +97 -33
  41. {hex_zmq_servers-0.3.2.dist-info → hex_zmq_servers-0.3.4.dist-info}/METADATA +7 -6
  42. {hex_zmq_servers-0.3.2.dist-info → hex_zmq_servers-0.3.4.dist-info}/RECORD +45 -35
  43. {hex_zmq_servers-0.3.2.dist-info → hex_zmq_servers-0.3.4.dist-info}/WHEEL +0 -0
  44. {hex_zmq_servers-0.3.2.dist-info → hex_zmq_servers-0.3.4.dist-info}/licenses/LICENSE +0 -0
  45. {hex_zmq_servers-0.3.2.dist-info → hex_zmq_servers-0.3.4.dist-info}/top_level.txt +0 -0
hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6_cli.py CHANGED
@@ -7,6 +7,7 @@
7
7
  ################################################################
8
8
 
9
9
  from ..mujoco_base import HexMujocoClientBase
10
+ from ...zmq_base import HexRate
10
11
 
11
12
  NET_CONFIG = {
12
13
  "ip": "127.0.0.1",
@@ -16,11 +17,52 @@ NET_CONFIG = {
16
17
  "server_num_workers": 4,
17
18
  }
18
19
 
20
+ RECV_CONFIG = {
21
+ "rgb": True,
22
+ "depth": True,
23
+ "obj": True,
24
+ }
25
+
19
26
 
20
27
  class HexMujocoArcherY6Client(HexMujocoClientBase):
21
28
 
22
29
  def __init__(
23
30
  self,
24
31
  net_config: dict = NET_CONFIG,
32
+ recv_config: dict = RECV_CONFIG,
25
33
  ):
26
34
  HexMujocoClientBase.__init__(self, net_config)
35
+ self.__recv_config = recv_config
36
+ self._wait_for_working()
37
+
38
+ def _recv_loop(self):
39
+ rate = HexRate(2000)
40
+ image_trig_cnt = 0
41
+ while self._recv_flag:
42
+ hdr, states = self._get_states_inner("robot")
43
+ if hdr is not None:
44
+ self._states_queue["robot"].append((hdr, states))
45
+ if self.__recv_config["obj"]:
46
+ hdr, obj_pose = self._get_states_inner("obj")
47
+ if hdr is not None:
48
+ self._states_queue["obj"].append((hdr, obj_pose))
49
+
50
+ image_trig_cnt += 1
51
+ if image_trig_cnt >= 10:
52
+ image_trig_cnt = 0
53
+ if self.__recv_config["rgb"]:
54
+ hdr, img = self._get_rgb_inner()
55
+ if hdr is not None:
56
+ self._rgb_queue.append((hdr, img))
57
+ if self.__recv_config["depth"]:
58
+ hdr, img = self._get_depth_inner()
59
+ if hdr is not None:
60
+ self._depth_queue.append((hdr, img))
61
+
62
+ try:
63
+ cmds = self._cmds_queue.popleft()
64
+ _ = self._set_cmds_inner(cmds)
65
+ except IndexError:
66
+ pass
67
+
68
+ rate.sleep()
hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6_srv.py CHANGED
@@ -7,7 +7,7 @@
7
7
  ################################################################
8
8
 
9
9
  import numpy as np
10
- from hex_zmq_servers.zmq_base import HexSafeValue
10
+ from collections import deque
11
11
 
12
12
  try:
13
13
  from ..mujoco_base import HexMujocoServerBase
@@ -31,7 +31,7 @@ NET_CONFIG = {
31
31
  }
32
32
 
33
33
  MUJOCO_CONFIG = {
34
- "states_rate": 500,
34
+ "states_rate": 1000,
35
35
  "img_rate": 30,
36
36
  "headless": False,
37
37
  "sens_ts": True,
@@ -51,19 +51,19 @@ class HexMujocoArcherY6Server(HexMujocoServerBase):
51
51
  self._device = HexMujocoArcherY6(params_config)
52
52
 
53
53
  # values
54
- self._states_robot_value = HexSafeValue()
55
- self._states_obj_value = HexSafeValue()
56
- self._rgb_value = HexSafeValue()
57
- self._depth_value = HexSafeValue()
54
+ self._states_robot_queue = deque(maxlen=10)
55
+ self._states_obj_queue = deque(maxlen=10)
56
+ self._rgb_queue = deque(maxlen=10)
57
+ self._depth_queue = deque(maxlen=10)
58
58
 
59
59
  def work_loop(self):
60
60
  try:
61
61
  self._device.work_loop([
62
- self._states_robot_value,
63
- self._states_obj_value,
64
- self._cmds_value,
65
- self._rgb_value,
66
- self._depth_value,
62
+ self._states_robot_queue,
63
+ self._states_obj_queue,
64
+ self._cmds_queue,
65
+ self._rgb_queue,
66
+ self._depth_queue,
67
67
  self._stop_event,
68
68
  ])
69
69
  finally:
@@ -79,15 +79,17 @@ class HexMujocoArcherY6Server(HexMujocoServerBase):
79
79
  # get robot name
80
80
  robot_name = recv_hdr["cmd"].split("_")[2]
81
81
  if robot_name == "robot":
82
- value = self._states_robot_value
82
+ queue = self._states_robot_queue
83
83
  elif robot_name == "obj":
84
- value = self._states_obj_value
84
+ queue = self._states_obj_queue
85
85
  else:
86
86
  raise ValueError(
87
87
  f"unknown robot name: {robot_name} in {recv_hdr['cmd']}")
88
88
 
89
89
  try:
90
- ts, count, states = value.get()
90
+ ts, count, states = queue.popleft()
91
+ except IndexError:
92
+ return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
91
93
  except Exception as e:
92
94
  print(f"\033[91m{recv_hdr['cmd']} failed: {e}\033[0m")
93
95
  return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
hex_zmq_servers/mujoco/e3_desktop/model/robot.xml CHANGED
@@ -10,37 +10,37 @@
10
10
  # left link 1
11
11
  <body name="left_link_1" pos="0.0185 0.3 0.21152">
12
12
  <inertial pos="0.008 -0.0023 0.0213" quat="0.707107 0.707107 0 0" mass="0.2154" diaginertia="0.0003 0.0003 0.0002"/>
13
- <joint name="left_joint_1" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
13
+ <joint name="left_joint_1" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" limited="true" armature="0.01" damping="0.1" frictionloss="0.5"/>
14
14
  <geom type="mesh" rgba="0.92549 0.92549 0.92549 1" mesh="arm_link_1"/>
15
15
 
16
16
  # left link 2
17
17
  <body name="left_link_2" pos="0.02 0 0.045">
18
18
  <inertial pos="0.0001 -0.0028 0.1317" quat="0.999789 0.0205404 0 0" mass="1.3123" diaginertia="0.018 0.0177288 0.000671225"/>
19
- <joint name="left_joint_2" pos="0 0 0" axis="0 1 0" range="-1.57 2.09" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
19
+ <joint name="left_joint_2" pos="0 0 0" axis="0 1 0" range="-1.57 2.09" limited="true" armature="0.01" damping="0.1" frictionloss="0.5"/>
20
20
  <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_2"/>
21
21
 
22
22
  # left link 3
23
23
  <body name="left_link_3" pos="0 0 0.264">
24
24
  <inertial pos="-0.051 0.0008 0.1447" quat="0.996909 0.0323182 -0.0688403 -0.019747" mass="1.1083" diaginertia="0.0115968 0.0112369 0.00136632"/>
25
- <joint name="left_joint_3" pos="0 0 0" axis="0 1 0" range="0 3.14" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
25
+ <joint name="left_joint_3" pos="0 0 0" axis="0 1 0" range="0 3.14" limited="true" armature="0.01" damping="0.1" frictionloss="0.5"/>
26
26
  <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_3"/>
27
27
 
28
28
  # left link 4
29
29
  <body name="left_link_4" pos="-0.06 0 0.245">
30
30
  <inertial pos="-0.0597 -0.001 0.0572" quat="0 0.92388 0 0.382683" mass="0.5398" diaginertia="0.0011 0.001 0.0005"/>
31
- <joint name="left_joint_4" pos="0 0 0" axis="0 1 0" range="-1.57 1.57" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
31
+ <joint name="left_joint_4" pos="0 0 0" axis="0 1 0" range="-1.57 1.57" limited="true" armature="0.01" damping="0.02" frictionloss="0.1"/>
32
32
  <geom type="mesh" rgba="0.792157 0.792157 0.792157 1" mesh="arm_link_4"/>
33
33
 
34
34
  # left link 5
35
35
  <body name="left_link_5" pos="-0.0553006 0 0.07025">
36
36
  <inertial pos="0.0292 0.0001 -0.0107" quat="0.707107 0 0 0.707107" mass="0.3984" diaginertia="0.0003 0.0002 0.0002"/>
37
- <joint name="left_joint_5" pos="0 0 0" axis="-1 0 0" range="-1.57 1.57" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
37
+ <joint name="left_joint_5" pos="0 0 0" axis="-1 0 0" range="-1.57 1.57" limited="true" armature="0.01" damping="0.02" frictionloss="0.1"/>
38
38
  <geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="arm_link_5"/>
39
39
 
40
40
  # left gripper base link
41
41
  <body name="left_gripper_base_link" pos="0.0578 0 0.02895" quat="0.707107 0.0 -0.707107 0.0">
42
42
  <inertial pos="-0.0139 0.0001 0.0578" quat="0.567843 0.421372 0.421372 0.567843" mass="0.7651" diaginertia="0.0021 0.00143028 0.00106972"/>
43
- <joint name="left_joint_6" pos="0 0 0" axis="1 0 0" range="-1.57 1.57" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
43
+ <joint name="left_joint_6" pos="0 0 0" axis="1 0 0" range="-1.57 1.57" limited="true" armature="0.01" damping="0.02" frictionloss="0.1"/>
44
44
  <geom type="mesh" rgba="1 1 1 1" mesh="gripper_base_link"/>
45
45
  <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" />
46
46
  <camera name="left_camera" pos="0.066 0 0.105" quat="0.8660254 0.0 -0.5 0.0" fovy="50"/>
@@ -99,37 +99,37 @@
99
99
  # right link 1
100
100
  <body name="right_link_1" pos="0.0185 -0.3 0.21152">
101
101
  <inertial pos="0.008 -0.0023 0.0213" quat="0.707107 0.707107 0 0" mass="0.2154" diaginertia="0.0003 0.0003 0.0002"/>
102
- <joint name="right_joint_1" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
102
+ <joint name="right_joint_1" pos="0 0 0" axis="0 0 1" range="-2.7 2.7" limited="true" armature="0.01" damping="0.1" frictionloss="0.5"/>
103
103
  <geom type="mesh" rgba="0.92549 0.92549 0.92549 1" mesh="arm_link_1"/>
104
104
 
105
105
  # right link 2
106
106
  <body name="right_link_2" pos="0.02 0 0.045">
107
107
  <inertial pos="0.0001 -0.0028 0.1317" quat="0.999789 0.0205404 0 0" mass="1.3123" diaginertia="0.018 0.0177288 0.000671225"/>
108
- <joint name="right_joint_2" pos="0 0 0" axis="0 1 0" range="-1.57 2.09" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
108
+ <joint name="right_joint_2" pos="0 0 0" axis="0 1 0" range="-1.57 2.09" limited="true" armature="0.01" damping="0.1" frictionloss="0.5"/>
109
109
  <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_2"/>
110
110
 
111
111
  # right link 3
112
112
  <body name="right_link_3" pos="0 0 0.264">
113
113
  <inertial pos="-0.051 0.0008 0.1447" quat="0.996909 0.0323182 -0.0688403 -0.019747" mass="1.1083" diaginertia="0.0115968 0.0112369 0.00136632"/>
114
- <joint name="right_joint_3" pos="0 0 0" axis="0 1 0" range="0 3.14" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
114
+ <joint name="right_joint_3" pos="0 0 0" axis="0 1 0" range="0 3.14" limited="true" armature="0.01" damping="0.1" frictionloss="0.5"/>
115
115
  <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_3"/>
116
116
 
117
117
  # right link 4
118
118
  <body name="right_link_4" pos="-0.06 0 0.245">
119
119
  <inertial pos="-0.0597 -0.001 0.0572" quat="0 0.92388 0 0.382683" mass="0.5398" diaginertia="0.0011 0.001 0.0005"/>
120
- <joint name="right_joint_4" pos="0 0 0" axis="0 1 0" range="-1.57 1.57" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
120
+ <joint name="right_joint_4" pos="0 0 0" axis="0 1 0" range="-1.57 1.57" limited="true" armature="0.01" damping="0.02" frictionloss="0.1"/>
121
121
  <geom type="mesh" rgba="0.792157 0.792157 0.792157 1" mesh="arm_link_4"/>
122
122
 
123
123
  # right link 5
124
124
  <body name="right_link_5" pos="-0.0553006 0 0.07025">
125
125
  <inertial pos="0.0292 0.0001 -0.0107" quat="0.707107 0 0 0.707107" mass="0.3984" diaginertia="0.0003 0.0002 0.0002"/>
126
- <joint name="right_joint_5" pos="0 0 0" axis="-1 0 0" range="-1.57 1.57" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
126
+ <joint name="right_joint_5" pos="0 0 0" axis="-1 0 0" range="-1.57 1.57" limited="true" armature="0.01" damping="0.02" frictionloss="0.1"/>
127
127
  <geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="arm_link_5"/>
128
128
 
129
129
  # right gripper base link
130
130
  <body name="right_gripper_base_link" pos="0.0578 0 0.02895" quat="0.707107 0.0 -0.707107 0.0">
131
131
  <inertial pos="-0.0139 0.0001 0.0578" quat="0.567843 0.421372 0.421372 0.567843" mass="0.7651" diaginertia="0.0021 0.00143028 0.00106972"/>
132
- <joint name="right_joint_6" pos="0 0 0" axis="1 0 0" range="-1.57 1.57" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
132
+ <joint name="right_joint_6" pos="0 0 0" axis="1 0 0" range="-1.57 1.57" limited="true" armature="0.01" damping="0.02" frictionloss="0.1"/>
133
133
  <geom type="mesh" rgba="1 1 1 1" mesh="gripper_base_link"/>
134
134
  <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" />
135
135
  <camera name="right_camera" pos="0.066 0 0.105" quat="0.8660254 0.0 -0.5 0.0" fovy="50"/>
hex_zmq_servers/mujoco/e3_desktop/model/scene.xml CHANGED
@@ -1,7 +1,7 @@
1
1
  <mujoco model="e3_desktop scene">
2
2
  <compiler angle="radian"/>
3
3
  <option integrator="implicitfast"/>
4
- <option timestep="2e-3"/>
4
+ <option timestep="1e-3"/>
5
5
 
6
6
  <statistic center="0.3 0 0.45" extent="0.8" meansize="0.05"/>
7
7
 
hex_zmq_servers/mujoco/e3_desktop/mujoco_e3_desktop.py CHANGED
@@ -11,29 +11,38 @@ import copy
11
11
  import threading
12
12
  import cv2
13
13
  import numpy as np
14
+ from collections import deque
14
15
 
15
16
  import mujoco
16
17
  from mujoco import viewer
17
18
 
18
19
  from ..mujoco_base import HexMujocoBase
19
20
  from ...zmq_base import (
21
+ hex_ns_now,
20
22
  hex_zmq_ts_now,
23
+ ns_to_hex_zmq_ts,
21
24
  hex_zmq_ts_delta_ms,
22
25
  HexRate,
23
- HexSafeValue,
24
26
  )
25
27
  from ...hex_launch import hex_log, HEX_LOG_LEVEL
26
28
  from hex_robo_utils import HexCtrlUtilMitJoint as CtrlUtil
27
29
 
28
30
  MUJOCO_CONFIG = {
29
- "states_rate": 500,
31
+ "states_rate": 1000,
30
32
  "img_rate": 30,
31
33
  "tau_ctrl": False,
32
34
  "mit_kp": [200.0, 200.0, 200.0, 75.0, 15.0, 15.0, 20.0],
33
35
  "mit_kd": [12.5, 12.5, 12.5, 6.0, 0.31, 0.31, 1.0],
36
+ "cam_type": ["empty", "empty", "empty"],
34
37
  "headless": False,
35
38
  "sens_ts": True,
36
39
  }
40
+ CAMERA_CONFIG = {
41
+ "empty": (False, False),
42
+ "rgb": (True, False),
43
+ "berxel": (True, True),
44
+ "realsense": (True, True),
45
+ }
37
46
 
38
47
 
39
48
  class HexMujocoE3Desktop(HexMujocoBase):
@@ -50,6 +59,7 @@ class HexMujocoE3Desktop(HexMujocoBase):
50
59
  self.__tau_ctrl = mujoco_config["tau_ctrl"]
51
60
  self.__mit_kp = mujoco_config["mit_kp"]
52
61
  self.__mit_kd = mujoco_config["mit_kd"]
62
+ self.__cam_type = mujoco_config["cam_type"]
53
63
  self.__headless = mujoco_config["headless"]
54
64
  self.__sens_ts = mujoco_config["sens_ts"]
55
65
  except KeyError as ke:
@@ -103,28 +113,44 @@ class HexMujocoE3Desktop(HexMujocoBase):
103
113
  self.__states_trig_thresh = int(self.__sim_rate / states_rate)
104
114
 
105
115
  # camera init
106
- width, height = 640, 400
116
+ self.__img_trig_thresh = int(self.__sim_rate / img_rate)
117
+ self.__width, self.__height = (640, 480)
107
118
  head_fovy_rad = self.__model.cam_fovy[0] * np.pi / 180.0
108
119
  left_fovy_rad = self.__model.cam_fovy[1] * np.pi / 180.0
109
120
  right_fovy_rad = self.__model.cam_fovy[2] * np.pi / 180.0
110
- head_focal = 0.5 * height / np.tan(head_fovy_rad / 2.0)
111
- left_focal = 0.5 * height / np.tan(left_fovy_rad / 2.0)
112
- right_focal = 0.5 * height / np.tan(right_fovy_rad / 2.0)
121
+ head_focal = 0.5 * self.__height / np.tan(head_fovy_rad / 2.0)
122
+ left_focal = 0.5 * self.__height / np.tan(left_fovy_rad / 2.0)
123
+ right_focal = 0.5 * self.__height / np.tan(right_fovy_rad / 2.0)
113
124
  self._intri = np.array([
114
- [head_focal, head_focal, height / 2, height / 2],
115
- [left_focal, left_focal, height / 2, height / 2],
116
- [right_focal, right_focal, height / 2, height / 2],
125
+ [head_focal, head_focal, self.__height / 2, self.__height / 2],
126
+ [left_focal, left_focal, self.__height / 2, self.__height / 2],
127
+ [right_focal, right_focal, self.__height / 2, self.__height / 2],
117
128
  ])
118
- self.__rgb_cam = mujoco.Renderer(self.__model, height, width)
119
- self.__depth_cam = mujoco.Renderer(self.__model, height, width)
120
- self.__depth_cam.enable_depth_rendering()
121
- self.__img_trig_thresh = int(self.__sim_rate / img_rate)
129
+ self.__head_rgb, self.__head_depth = CAMERA_CONFIG.get(
130
+ self.__cam_type[0], (False, False))
131
+ self.__left_rgb, self.__left_depth = CAMERA_CONFIG.get(
132
+ self.__cam_type[1], (False, False))
133
+ self.__right_rgb, self.__right_depth = CAMERA_CONFIG.get(
134
+ self.__cam_type[2], (False, False))
135
+ self.__rgb_cam, self.__depth_cam = None, None
136
+ has_rgb = self.__left_rgb or self.__right_rgb or self.__head_rgb
137
+ has_depth = self.__left_depth or self.__right_depth or self.__head_depth
138
+ if has_rgb:
139
+ self.__rgb_cam = mujoco.Renderer(self.__model, self.__height,
140
+ self.__width)
141
+ if has_depth:
142
+ self.__depth_cam = mujoco.Renderer(self.__model, self.__height,
143
+ self.__width)
144
+ self.__depth_cam.enable_depth_rendering()
122
145
 
123
146
  # viewer init
124
147
  mujoco.mj_forward(self.__model, self.__data)
125
148
  if not self.__headless:
126
149
  self.__viewer = viewer.launch_passive(self.__model, self.__data)
127
150
 
151
+ # time init
152
+ self.__bias_ns = hex_ns_now() - self.__data.time * 1_000_000_000
153
+
128
154
  # start work loop
129
155
  self._working.set()
130
156
 
@@ -140,19 +166,19 @@ class HexMujocoE3Desktop(HexMujocoBase):
140
166
  self.__viewer.sync()
141
167
  return True
142
168
 
143
- def work_loop(self, hex_values: list[HexSafeValue | threading.Event]):
144
- states_left_value = hex_values[0]
145
- states_right_value = hex_values[1]
146
- states_obj_value = hex_values[2]
147
- cmds_left_value = hex_values[3]
148
- cmds_right_value = hex_values[4]
149
- head_rgb_value = hex_values[5]
150
- head_depth_value = hex_values[6]
151
- left_rgb_value = hex_values[7]
152
- left_depth_value = hex_values[8]
153
- right_rgb_value = hex_values[9]
154
- right_depth_value = hex_values[10]
155
- stop_event = hex_values[11]
169
+ def work_loop(self, hex_queues: list[deque | threading.Event]):
170
+ states_left_queue = hex_queues[0]
171
+ states_right_queue = hex_queues[1]
172
+ states_obj_queue = hex_queues[2]
173
+ cmds_left_queue = hex_queues[3]
174
+ cmds_right_queue = hex_queues[4]
175
+ head_rgb_queue = hex_queues[5]
176
+ head_depth_queue = hex_queues[6]
177
+ left_rgb_queue = hex_queues[7]
178
+ left_depth_queue = hex_queues[8]
179
+ right_rgb_queue = hex_queues[9]
180
+ right_depth_queue = hex_queues[10]
181
+ stop_event = hex_queues[11]
156
182
 
157
183
  last_states_ts = {"s": 0, "ns": 0}
158
184
  states_left_count = 0
@@ -166,10 +192,31 @@ class HexMujocoE3Desktop(HexMujocoBase):
166
192
  left_depth_count = 0
167
193
  right_rgb_count = 0
168
194
  right_depth_count = 0
195
+ cmds_left = None
196
+ cmds_right = None
169
197
 
170
198
  rate = HexRate(self.__sim_rate)
171
199
  states_trig_count = 0
172
200
  img_trig_count = 0
201
+ init_ts = self.__mujoco_ts() if self.__sens_ts else hex_zmq_ts_now()
202
+ head_rgb_queue.append((init_ts, 0,
203
+ np.zeros((self.__height, self.__width, 3),
204
+ dtype=np.uint8)))
205
+ head_depth_queue.append((init_ts, 0,
206
+ np.zeros((self.__height, self.__width),
207
+ dtype=np.uint16)))
208
+ left_rgb_queue.append((init_ts, 0,
209
+ np.zeros((self.__height, self.__width, 3),
210
+ dtype=np.uint8)))
211
+ left_depth_queue.append((init_ts, 0,
212
+ np.zeros((self.__height, self.__width),
213
+ dtype=np.uint16)))
214
+ right_rgb_queue.append((init_ts, 0,
215
+ np.zeros((self.__height, self.__width, 3),
216
+ dtype=np.uint8)))
217
+ right_depth_queue.append((init_ts, 0,
218
+ np.zeros((self.__height, self.__width),
219
+ dtype=np.uint16)))
173
220
  while self._working.is_set() and not stop_event.is_set():
174
221
  states_trig_count += 1
175
222
  if states_trig_count >= self.__states_trig_thresh:
@@ -178,83 +225,105 @@ class HexMujocoE3Desktop(HexMujocoBase):
178
225
  # states
179
226
  ts, states_left, states_right, states_obj = self.__get_states()
180
227
  if states_left is not None:
181
- if hex_zmq_ts_delta_ms(ts, last_states_ts) > 1.0:
228
+ if hex_zmq_ts_delta_ms(ts, last_states_ts) > 1e-6:
182
229
  last_states_ts = ts
183
230
  # states left
184
- states_left_value.set(
231
+ states_left_queue.append(
185
232
  (ts, states_left_count, states_left))
186
233
  states_left_count = (states_left_count +
187
234
  1) % self._max_seq_num
188
235
  # states right
189
- states_right_value.set(
236
+ states_right_queue.append(
190
237
  (ts, states_right_count, states_right))
191
238
  states_right_count = (states_right_count +
192
239
  1) % self._max_seq_num
193
240
  # states obj
194
- states_obj_value.set(
241
+ states_obj_queue.append(
195
242
  (ts, states_obj_count, states_obj))
196
243
  states_obj_count = (states_obj_count +
197
244
  1) % self._max_seq_num
198
245
 
199
246
  # cmds
200
- cmds_left_pack = cmds_left_value.get(timeout_s=-1.0)
247
+ cmds_left_pack = None
248
+ try:
249
+ cmds_left_pack = cmds_left_queue.popleft()
250
+ except IndexError:
251
+ pass
201
252
  if cmds_left_pack is not None:
202
253
  ts, seq, cmds_left = cmds_left_pack
203
254
  if seq != last_cmds_left_seq:
204
255
  last_cmds_left_seq = seq
205
256
  if hex_zmq_ts_delta_ms(hex_zmq_ts_now(), ts) < 200.0:
206
- self.__set_cmds(cmds_left, "left")
207
-
208
- cmds_right_pack = cmds_right_value.get(timeout_s=-1.0)
257
+ cmds_left = cmds_left.copy()
258
+ if cmds_left is not None:
259
+ self.__set_cmds(cmds_left, "left")
260
+
261
+ cmds_right_pack = None
262
+ try:
263
+ cmds_right_pack = cmds_right_queue.popleft()
264
+ except IndexError:
265
+ pass
209
266
  if cmds_right_pack is not None:
210
- ts, seq, cmds_right = cmds_right_pack
267
+ ts, seq, cmds_right_get = cmds_right_pack
211
268
  if seq != last_cmds_right_seq:
212
269
  last_cmds_right_seq = seq
213
270
  if hex_zmq_ts_delta_ms(hex_zmq_ts_now(), ts) < 200.0:
214
- self.__set_cmds(cmds_right, "right")
271
+ cmds_right = cmds_right_get.copy()
272
+ if cmds_right is not None:
273
+ self.__set_cmds(cmds_right, "right")
215
274
 
216
275
  img_trig_count += 1
217
276
  if img_trig_count >= self.__img_trig_thresh:
218
277
  img_trig_count = 0
219
278
 
220
279
  # head rgb
221
- ts, rgb_img = self.__get_rgb("head_camera")
222
- if rgb_img is not None:
223
- head_rgb_value.set((ts, head_rgb_count, rgb_img))
224
- head_rgb_count = (head_rgb_count + 1) % self._max_seq_num
280
+ if self.__head_rgb:
281
+ ts, rgb_img = self.__get_rgb("head_camera")
282
+ if rgb_img is not None:
283
+ head_rgb_queue.append((ts, head_rgb_count, rgb_img))
284
+ head_rgb_count = (head_rgb_count +
285
+ 1) % self._max_seq_num
225
286
 
226
287
  # head depth
227
- ts, depth_img = self.__get_depth("head_camera")
228
- if depth_img is not None:
229
- head_depth_value.set((ts, head_depth_count, depth_img))
230
- head_depth_count = (head_depth_count +
231
- 1) % self._max_seq_num
288
+ if self.__head_depth:
289
+ ts, depth_img = self.__get_depth("head_camera")
290
+ if depth_img is not None:
291
+ head_depth_queue.append((ts, head_depth_count, depth_img))
292
+ head_depth_count = (head_depth_count +
293
+ 1) % self._max_seq_num
232
294
 
233
295
  # left rgb
234
- ts, rgb_img = self.__get_rgb("left_camera")
235
- if rgb_img is not None:
236
- left_rgb_value.set((ts, left_rgb_count, rgb_img))
237
- left_rgb_count = (left_rgb_count + 1) % self._max_seq_num
296
+ if self.__left_rgb:
297
+ ts, rgb_img = self.__get_rgb("left_camera")
298
+ if rgb_img is not None:
299
+ left_rgb_queue.append((ts, left_rgb_count, rgb_img))
300
+ left_rgb_count = (left_rgb_count +
301
+ 1) % self._max_seq_num
238
302
 
239
303
  # left depth
240
- ts, depth_img = self.__get_depth("left_camera")
241
- if depth_img is not None:
242
- left_depth_value.set((ts, left_depth_count, depth_img))
243
- left_depth_count = (left_depth_count +
244
- 1) % self._max_seq_num
304
+ if self.__left_depth:
305
+ ts, depth_img = self.__get_depth("left_camera")
306
+ if depth_img is not None:
307
+ left_depth_queue.append((ts, left_depth_count, depth_img))
308
+ left_depth_count = (left_depth_count +
309
+ 1) % self._max_seq_num
245
310
 
246
311
  # right rgb
247
- ts, rgb_img = self.__get_rgb("right_camera")
248
- if rgb_img is not None:
249
- right_rgb_value.set((ts, right_rgb_count, rgb_img))
250
- right_rgb_count = (right_rgb_count + 1) % self._max_seq_num
312
+ if self.__right_rgb:
313
+ ts, rgb_img = self.__get_rgb("right_camera")
314
+ if rgb_img is not None:
315
+ right_rgb_queue.append((ts, right_rgb_count, rgb_img))
316
+ right_rgb_count = (right_rgb_count +
317
+ 1) % self._max_seq_num
251
318
 
252
319
  # right depth
253
- ts, depth_img = self.__get_depth("right_camera")
254
- if depth_img is not None:
255
- right_depth_value.set((ts, right_depth_count, depth_img))
256
- right_depth_count = (right_depth_count +
257
- 1) % self._max_seq_num
320
+ if self.__right_depth:
321
+ ts, depth_img = self.__get_depth("right_camera")
322
+ if depth_img is not None:
323
+ right_depth_queue.append(
324
+ (ts, right_depth_count, depth_img))
325
+ right_depth_count = (right_depth_count +
326
+ 1) % self._max_seq_num
258
327
 
259
328
  # mujoco step
260
329
  mujoco.mj_step(self.__model, self.__data)
@@ -359,18 +428,17 @@ class HexMujocoE3Desktop(HexMujocoBase):
359
428
  ), depth_img
360
429
 
361
430
  def __mujoco_ts(self):
362
- mujoco_ts = self.__data.time
363
- return {
364
- "s": int(mujoco_ts // 1),
365
- "ns": int((mujoco_ts % 1) * 1_000_000_000),
366
- }
431
+ mujoco_ts = self.__data.time * 1_000_000_000 + self.__bias_ns
432
+ return ns_to_hex_zmq_ts(mujoco_ts)
367
433
 
368
434
  def close(self):
369
435
  if not self._working.is_set():
370
436
  return
371
437
  self._working.clear()
372
- self.__rgb_cam.close()
373
- self.__depth_cam.close()
438
+ if self.__rgb_cam is not None:
439
+ self.__rgb_cam.close()
440
+ if self.__depth_cam is not None:
441
+ self.__depth_cam.close()
374
442
  if not self.__headless:
375
443
  self.__viewer.close()
376
444
  hex_log(HEX_LOG_LEVEL["info"], "HexMujocoE3Desktop closed")