PyPI - hex-zmq-servers - Versions diffs - 0.3.2__py3-none-any.whl → 0.3.3__py3-none-any.whl - Mend

hex-zmq-servers 0.3.2py3-none-any.whl → 0.3.3py3-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 (24) hide show

hex_zmq_servers/__init__.py +40 -17
hex_zmq_servers/cam/__init__.py +29 -5
hex_zmq_servers/cam/realsense/__init__.py +17 -0
hex_zmq_servers/cam/realsense/cam_realsense.py +157 -0
hex_zmq_servers/cam/realsense/cam_realsense_cli.py +30 -0
hex_zmq_servers/cam/realsense/cam_realsense_srv.py +75 -0
hex_zmq_servers/cam/rgb/__init__.py +17 -0
hex_zmq_servers/cam/rgb/cam_rgb.py +107 -0
hex_zmq_servers/cam/rgb/cam_rgb_cli.py +30 -0
hex_zmq_servers/cam/rgb/cam_rgb_srv.py +75 -0
hex_zmq_servers/config/cam_realsense.json +15 -0
hex_zmq_servers/config/cam_rgb.json +18 -0
hex_zmq_servers/config/mujoco_archer_y6.json +1 -0
hex_zmq_servers/config/mujoco_e3_desktop.json +5 -0
hex_zmq_servers/mujoco/archer_y6/model/robot.xml +6 -6
hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6.py +50 -21
hex_zmq_servers/mujoco/e3_desktop/model/robot.xml +12 -12
hex_zmq_servers/mujoco/e3_desktop/mujoco_e3_desktop.py +102 -44
hex_zmq_servers/zmq_base.py +5 -2
{hex_zmq_servers-0.3.2.dist-info → hex_zmq_servers-0.3.3.dist-info}/METADATA +7 -6
{hex_zmq_servers-0.3.2.dist-info → hex_zmq_servers-0.3.3.dist-info}/RECORD +24 -14
{hex_zmq_servers-0.3.2.dist-info → hex_zmq_servers-0.3.3.dist-info}/WHEEL +0 -0
{hex_zmq_servers-0.3.2.dist-info → hex_zmq_servers-0.3.3.dist-info}/licenses/LICENSE +0 -0
{hex_zmq_servers-0.3.2.dist-info → hex_zmq_servers-0.3.3.dist-info}/top_level.txt +0 -0

hex_zmq_servers/config/cam_rgb.json ADDED Viewed

@@ -0,0 +1,18 @@
+{
+    "net": {
+        "ip": "127.0.0.1",
+        "port": 12345,
+        "client_timeout_ms": 200,
+        "server_timeout_ms": 1000,
+        "server_num_workers": 4
+    },
+    "params": {
+        "cam_path": "/dev/video0",
+        "resolution": [
+            640,
+            480
+        ],
+        "frame_rate": 30,
+        "sens_ts": true
+    }
+}

hex_zmq_servers/config/mujoco_archer_y6.json CHANGED Viewed

@@ -28,6 +28,7 @@
             0.31,
             1.0
         ],
+        "cam_type": "empty",
         "headless": false,
         "sens_ts": true
     }

hex_zmq_servers/config/mujoco_e3_desktop.json CHANGED Viewed

@@ -28,6 +28,11 @@
             0.31,
             1.0
         ],
+        "cam_type": [
+            "empty",
+            "empty",
+            "empty"
+        ],
         "headless": false,
         "sens_ts": true
     }

hex_zmq_servers/mujoco/archer_y6/model/robot.xml CHANGED Viewed

@@ -6,37 +6,37 @@
         # link 1
         <body name="link_1" pos="0 0 0.0665">
             <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"/>
-            <joint name="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"/>
+            <joint name="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"/>
             <geom type="mesh" rgba="0.92549 0.92549 0.92549 1" mesh="arm_link_1"/>
             # link 2
             <body name="link_2" pos="0.02 0 0.045">
                 <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"/>
-                <joint name="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"/>
+                <joint name="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"/>
                 <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_2"/>
                 # link 3
                 <body name="link_3" pos="0 0 0.264">
                     <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"/>
-                    <joint name="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"/>
+                    <joint name="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"/>
                     <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_3"/>
                     # link 4
                     <body name="link_4" pos="-0.06 0 0.245">
                         <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"/>
-                        <joint name="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"/>
+                        <joint name="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"/>
                         <geom type="mesh" rgba="0.792157 0.792157 0.792157 1" mesh="arm_link_4"/>
                         # link 5
                         <body name="link_5" pos="-0.0553 0 0.070">
                             <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"/>
-                            <joint name="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"/>
+                            <joint name="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"/>
                             <geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="arm_link_5"/>
                             # gripper base link
                             <body name="gripper_base_link" pos="0.0553 0 0.029" quat="0.707107 0.0 -0.707107 0.0">
                                 <inertial pos="-0.0139 0.0001 0.0553" quat="0.567843 0.421372 0.421372 0.567843" mass="0.7651" diaginertia="0.0021 0.00143028 0.00106972"/>
-                                <joint name="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"/>
+                                <joint name="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"/>
                                 <geom type="mesh" rgba="1 1 1 1" mesh="gripper_base_link"/>
                                 <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"/>

hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6.py CHANGED Viewed

@@ -34,6 +34,12 @@ MUJOCO_CONFIG = {
     "headless": False,
     "sens_ts": True,
 }
+CAMERA_CONFIG = {
+    "empty": (False, False),
+    "rgb": (True, False),
+    "berxel": (True, True),
+    "realsense": (True, True),
+}
 class HexMujocoArcherY6(HexMujocoBase):
@@ -50,6 +56,7 @@ class HexMujocoArcherY6(HexMujocoBase):
             self.__tau_ctrl = mujoco_config["tau_ctrl"]
             self.__mit_kp = mujoco_config["mit_kp"]
             self.__mit_kd = mujoco_config["mit_kd"]
+            self.__cam_type = mujoco_config["cam_type"]
             self.__headless = mujoco_config["headless"]
             self.__sens_ts = mujoco_config["sens_ts"]
         except KeyError as ke:
@@ -95,14 +102,22 @@ class HexMujocoArcherY6(HexMujocoBase):
         self.__states_trig_thresh = int(self.__sim_rate / states_rate)
         # camera init
-        width, height = 640, 400
-        fovy_rad = self.__model.cam_fovy[0] * np.pi / 180.0
-        focal = 0.5 * height / np.tan(fovy_rad / 2.0)
-        self._intri = np.array([focal, focal, height / 2, height / 2])
-        self.__rgb_cam = mujoco.Renderer(self.__model, height, width)
-        self.__depth_cam = mujoco.Renderer(self.__model, height, width)
-        self.__depth_cam.enable_depth_rendering()
         self.__img_trig_thresh = int(self.__sim_rate / img_rate)
+        self.__width, self.__height = 640, 400
+        fovy_rad = self.__model.cam_fovy[0] * np.pi / 180.0
+        focal = 0.5 * self.__height / np.tan(fovy_rad / 2.0)
+        self._intri = np.array(
+            [focal, focal, self.__height / 2, self.__height / 2])
+        self.__use_rgb, self.__use_depth = CAMERA_CONFIG.get(
+            self.__cam_type, (False, False))
+        self.__rgb_cam, self.__depth_cam = None, None
+        if self.__use_rgb:
+            self.__rgb_cam = mujoco.Renderer(self.__model, self.__height,
+                                             self.__width)
+        if self.__use_depth:
+            self.__depth_cam = mujoco.Renderer(self.__model, self.__height,
+                                               self.__width)
+            self.__depth_cam.enable_depth_rendering()
         # viewer init
         mujoco.mj_forward(self.__model, self.__data)
@@ -138,11 +153,18 @@ class HexMujocoArcherY6(HexMujocoBase):
         last_cmds_robot_seq = -1
         rgb_count = 0
         depth_count = 0
+        cmds_robot = None
         rate = HexRate(self.__sim_rate)
         states_trig_count = 0
         img_trig_count = 0
+        init_ts = self.__mujoco_ts() if self.__sens_ts else hex_zmq_ts_now()
+        rgb_value.set((init_ts, 0,
+                       np.zeros((self.__height, self.__width, 3),
+                                dtype=np.uint8)))
+        depth_value.set((init_ts, 0,
+                         np.zeros((self.__height, self.__width),
+                                  dtype=np.uint16)))
         while self._working.is_set() and not stop_event.is_set():
             states_trig_count += 1
             if states_trig_count >= self.__states_trig_thresh:
@@ -169,27 +191,31 @@ class HexMujocoArcherY6(HexMujocoBase):
                 # cmds
                 cmds_robot_pack = cmds_robot_value.get(timeout_s=-1.0)
                 if cmds_robot_pack is not None:
-                    ts, seq, cmds = cmds_robot_pack
+                    ts, seq, cmds_robot_get = cmds_robot_pack
                     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)
+                            cmds_robot = cmds_robot_get.copy()
+                if cmds_robot is not None:
+                    self.__set_cmds(cmds_robot)
             img_trig_count += 1
             if img_trig_count >= self.__img_trig_thresh:
                 img_trig_count = 0
                 # rgb
-                ts, rgb_img = self.__get_rgb()
-                if rgb_img is not None:
-                    rgb_value.set((ts, rgb_count, rgb_img))
-                    rgb_count = (rgb_count + 1) % self._max_seq_num
+                if self.__use_rgb:
+                    ts, rgb_img = self.__get_rgb()
+                    if rgb_img is not None:
+                        rgb_value.set((ts, rgb_count, rgb_img))
+                        rgb_count = (rgb_count + 1) % self._max_seq_num
                 # depth
-                ts, depth_img = self.__get_depth()
-                if depth_img is not None:
-                    depth_value.set((ts, depth_count, depth_img))
-                    depth_count = (depth_count + 1) % self._max_seq_num
+                if self.__use_depth:
+                    ts, depth_img = self.__get_depth()
+                    if depth_img is not None:
+                        depth_value.set((ts, depth_count, depth_img))
+                        depth_count = (depth_count + 1) % self._max_seq_num
             # mujoco step
             mujoco.mj_step(self.__model, self.__data)
@@ -236,7 +262,8 @@ class HexMujocoArcherY6(HexMujocoBase):
                     cmd_kp = cmds[:, 3].copy()
                     cmd_kd = cmds[:, 4].copy()
                 else:
-                    raise ValueError(f"The shape of cmds is invalid: {cmds.shape}")
+                    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(
@@ -282,8 +309,10 @@ class HexMujocoArcherY6(HexMujocoBase):
         if not self._working.is_set():
             return
         self._working.clear()
-        self.__rgb_cam.close()
-        self.__depth_cam.close()
+        if self.__rgb_cam is not None:
+            self.__rgb_cam.close()
+        if self.__depth_cam is not None:
+            self.__depth_cam.close()
         if not self.__headless:
             self.__viewer.close()
         hex_log(HEX_LOG_LEVEL["info"], "HexMujocoArcherY6 closed")

hex_zmq_servers/mujoco/e3_desktop/model/robot.xml CHANGED Viewed

@@ -10,37 +10,37 @@
         # left link 1
         <body name="left_link_1" pos="0.0185 0.3 0.21152">
             <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"/>
-            <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"/>
+            <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"/>
             <geom type="mesh" rgba="0.92549 0.92549 0.92549 1" mesh="arm_link_1"/>
             # left link 2
             <body name="left_link_2" pos="0.02 0 0.045">
                 <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"/>
-                <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"/>
+                <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"/>
                 <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_2"/>
                 # left link 3
                 <body name="left_link_3" pos="0 0 0.264">
                     <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"/>
-                    <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"/>
+                    <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"/>
                     <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_3"/>
                     # left link 4
                     <body name="left_link_4" pos="-0.06 0 0.245">
                         <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"/>
-                        <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"/>
+                        <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"/>
                         <geom type="mesh" rgba="0.792157 0.792157 0.792157 1" mesh="arm_link_4"/>
                         # left link 5
                         <body name="left_link_5" pos="-0.0553006 0 0.07025">
                             <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"/>
-                            <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"/>
+                            <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"/>
                             <geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="arm_link_5"/>
                             # left gripper base link
                             <body name="left_gripper_base_link" pos="0.0578 0 0.02895" quat="0.707107 0.0 -0.707107 0.0">
                                 <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"/>
-                                <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"/>
+                                <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"/>
                                 <geom type="mesh" rgba="1 1 1 1" mesh="gripper_base_link"/>
                                 <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="left_camera" pos="0.066 0 0.105" quat="0.8660254 0.0 -0.5 0.0" fovy="50"/>
@@ -99,37 +99,37 @@
         # right link 1
         <body name="right_link_1" pos="0.0185 -0.3 0.21152">
             <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"/>
-            <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"/>
+            <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"/>
             <geom type="mesh" rgba="0.92549 0.92549 0.92549 1" mesh="arm_link_1"/>
             # right link 2
             <body name="right_link_2" pos="0.02 0 0.045">
                 <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"/>
-                <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"/>
+                <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"/>
                 <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_2"/>
                 # right link 3
                 <body name="right_link_3" pos="0 0 0.264">
                     <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"/>
-                    <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"/>
+                    <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"/>
                     <geom type="mesh" rgba="1 1 1 1" mesh="arm_link_3"/>
                     # right link 4
                     <body name="right_link_4" pos="-0.06 0 0.245">
                         <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"/>
-                        <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"/>
+                        <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"/>
                         <geom type="mesh" rgba="0.792157 0.792157 0.792157 1" mesh="arm_link_4"/>
                         # right link 5
                         <body name="right_link_5" pos="-0.0553006 0 0.07025">
                             <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"/>
-                            <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"/>
+                            <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"/>
                             <geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="arm_link_5"/>
                             # right gripper base link
                             <body name="right_gripper_base_link" pos="0.0578 0 0.02895" quat="0.707107 0.0 -0.707107 0.0">
                                 <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"/>
-                                <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"/>
+                                <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"/>
                                 <geom type="mesh" rgba="1 1 1 1" mesh="gripper_base_link"/>
                                 <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="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/mujoco_e3_desktop.py CHANGED Viewed

@@ -31,9 +31,16 @@ MUJOCO_CONFIG = {
     "tau_ctrl": False,
     "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],
+    "cam_type": ["empty", "empty", "empty"],
     "headless": False,
     "sens_ts": True,
 }
+CAMERA_CONFIG = {
+    "empty": (False, False),
+    "rgb": (True, False),
+    "berxel": (True, True),
+    "realsense": (True, True),
+}
 class HexMujocoE3Desktop(HexMujocoBase):
@@ -50,6 +57,7 @@ class HexMujocoE3Desktop(HexMujocoBase):
             self.__tau_ctrl = mujoco_config["tau_ctrl"]
             self.__mit_kp = mujoco_config["mit_kp"]
             self.__mit_kd = mujoco_config["mit_kd"]
+            self.__cam_type = mujoco_config["cam_type"]
             self.__headless = mujoco_config["headless"]
             self.__sens_ts = mujoco_config["sens_ts"]
         except KeyError as ke:
@@ -103,22 +111,35 @@ class HexMujocoE3Desktop(HexMujocoBase):
         self.__states_trig_thresh = int(self.__sim_rate / states_rate)
         # camera init
-        width, height = 640, 400
+        self.__img_trig_thresh = int(self.__sim_rate / img_rate)
+        self.__width, self.__height = (640, 480)
         head_fovy_rad = self.__model.cam_fovy[0] * np.pi / 180.0
         left_fovy_rad = self.__model.cam_fovy[1] * np.pi / 180.0
         right_fovy_rad = self.__model.cam_fovy[2] * np.pi / 180.0
-        head_focal = 0.5 * height / np.tan(head_fovy_rad / 2.0)
-        left_focal = 0.5 * height / np.tan(left_fovy_rad / 2.0)
-        right_focal = 0.5 * height / np.tan(right_fovy_rad / 2.0)
+        head_focal = 0.5 * self.__height / np.tan(head_fovy_rad / 2.0)
+        left_focal = 0.5 * self.__height / np.tan(left_fovy_rad / 2.0)
+        right_focal = 0.5 * self.__height / np.tan(right_fovy_rad / 2.0)
         self._intri = np.array([
-            [head_focal, head_focal, height / 2, height / 2],
-            [left_focal, left_focal, height / 2, height / 2],
-            [right_focal, right_focal, height / 2, height / 2],
+            [head_focal, head_focal, self.__height / 2, self.__height / 2],
+            [left_focal, left_focal, self.__height / 2, self.__height / 2],
+            [right_focal, right_focal, self.__height / 2, self.__height / 2],
         ])
-        self.__rgb_cam = mujoco.Renderer(self.__model, height, width)
-        self.__depth_cam = mujoco.Renderer(self.__model, height, width)
-        self.__depth_cam.enable_depth_rendering()
-        self.__img_trig_thresh = int(self.__sim_rate / img_rate)
+        self.__head_rgb, self.__head_depth = CAMERA_CONFIG.get(
+            self.__cam_type[0], (False, False))
+        self.__left_rgb, self.__left_depth = CAMERA_CONFIG.get(
+            self.__cam_type[1], (False, False))
+        self.__right_rgb, self.__right_depth = CAMERA_CONFIG.get(
+            self.__cam_type[2], (False, False))
+        self.__rgb_cam, self.__depth_cam = None, None
+        has_rgb = self.__left_rgb or self.__right_rgb or self.__head_rgb
+        has_depth = self.__left_depth or self.__right_depth or self.__head_depth
+        if has_rgb:
+            self.__rgb_cam = mujoco.Renderer(self.__model, self.__height,
+                                             self.__width)
+        if has_depth:
+            self.__depth_cam = mujoco.Renderer(self.__model, self.__height,
+                                               self.__width)
+            self.__depth_cam.enable_depth_rendering()
         # viewer init
         mujoco.mj_forward(self.__model, self.__data)
@@ -166,10 +187,31 @@ class HexMujocoE3Desktop(HexMujocoBase):
         left_depth_count = 0
         right_rgb_count = 0
         right_depth_count = 0
+        cmds_left = None
+        cmds_right = None
         rate = HexRate(self.__sim_rate)
         states_trig_count = 0
         img_trig_count = 0
+        init_ts = self.__mujoco_ts() if self.__sens_ts else hex_zmq_ts_now()
+        head_rgb_value.set((init_ts, 0,
+                            np.zeros((self.__height, self.__width, 3),
+                                     dtype=np.uint8)))
+        head_depth_value.set((init_ts, 0,
+                              np.zeros((self.__height, self.__width),
+                                       dtype=np.uint16)))
+        left_rgb_value.set((init_ts, 0,
+                            np.zeros((self.__height, self.__width, 3),
+                                     dtype=np.uint8)))
+        left_depth_value.set((init_ts, 0,
+                              np.zeros((self.__height, self.__width),
+                                       dtype=np.uint16)))
+        right_rgb_value.set((init_ts, 0,
+                             np.zeros((self.__height, self.__width, 3),
+                                      dtype=np.uint8)))
+        right_depth_value.set((init_ts, 0,
+                               np.zeros((self.__height, self.__width),
+                                        dtype=np.uint16)))
         while self._working.is_set() and not stop_event.is_set():
             states_trig_count += 1
             if states_trig_count >= self.__states_trig_thresh:
@@ -199,62 +241,76 @@ class HexMujocoE3Desktop(HexMujocoBase):
                 # cmds
                 cmds_left_pack = cmds_left_value.get(timeout_s=-1.0)
                 if cmds_left_pack is not None:
-                    ts, seq, cmds_left = cmds_left_pack
+                    ts, seq, cmds_left_get = cmds_left_pack
                     if seq != last_cmds_left_seq:
                         last_cmds_left_seq = seq
                         if hex_zmq_ts_delta_ms(hex_zmq_ts_now(), ts) < 200.0:
-                            self.__set_cmds(cmds_left, "left")
+                            cmds_left = cmds_left_get.copy()
+                if cmds_left is not None:
+                    self.__set_cmds(cmds_left, "left")
                 cmds_right_pack = cmds_right_value.get(timeout_s=-1.0)
                 if cmds_right_pack is not None:
-                    ts, seq, cmds_right = cmds_right_pack
+                    ts, seq, cmds_right_get = cmds_right_pack
                     if seq != last_cmds_right_seq:
                         last_cmds_right_seq = seq
                         if hex_zmq_ts_delta_ms(hex_zmq_ts_now(), ts) < 200.0:
-                            self.__set_cmds(cmds_right, "right")
+                            cmds_right = cmds_right_get.copy()
+                if cmds_right is not None:
+                    self.__set_cmds(cmds_right, "right")
             img_trig_count += 1
             if img_trig_count >= self.__img_trig_thresh:
                 img_trig_count = 0
                 # head rgb
-                ts, rgb_img = self.__get_rgb("head_camera")
-                if rgb_img is not None:
-                    head_rgb_value.set((ts, head_rgb_count, rgb_img))
-                    head_rgb_count = (head_rgb_count + 1) % self._max_seq_num
+                if self.__head_rgb:
+                    ts, rgb_img = self.__get_rgb("head_camera")
+                    if rgb_img is not None:
+                        head_rgb_value.set((ts, head_rgb_count, rgb_img))
+                        head_rgb_count = (head_rgb_count +
+                                          1) % self._max_seq_num
                 # head depth
-                ts, depth_img = self.__get_depth("head_camera")
-                if depth_img is not None:
-                    head_depth_value.set((ts, head_depth_count, depth_img))
-                    head_depth_count = (head_depth_count +
-                                        1) % self._max_seq_num
+                if self.__head_depth:
+                    ts, depth_img = self.__get_depth("head_camera")
+                    if depth_img is not None:
+                        head_depth_value.set((ts, head_depth_count, depth_img))
+                        head_depth_count = (head_depth_count +
+                                            1) % self._max_seq_num
                 # left rgb
-                ts, rgb_img = self.__get_rgb("left_camera")
-                if rgb_img is not None:
-                    left_rgb_value.set((ts, left_rgb_count, rgb_img))
-                    left_rgb_count = (left_rgb_count + 1) % self._max_seq_num
+                if self.__left_rgb:
+                    ts, rgb_img = self.__get_rgb("left_camera")
+                    if rgb_img is not None:
+                        left_rgb_value.set((ts, left_rgb_count, rgb_img))
+                        left_rgb_count = (left_rgb_count +
+                                          1) % self._max_seq_num
                 # left depth
-                ts, depth_img = self.__get_depth("left_camera")
-                if depth_img is not None:
-                    left_depth_value.set((ts, left_depth_count, depth_img))
-                    left_depth_count = (left_depth_count +
-                                        1) % self._max_seq_num
+                if self.__left_depth:
+                    ts, depth_img = self.__get_depth("left_camera")
+                    if depth_img is not None:
+                        left_depth_value.set((ts, left_depth_count, depth_img))
+                        left_depth_count = (left_depth_count +
+                                            1) % self._max_seq_num
                 # right rgb
-                ts, rgb_img = self.__get_rgb("right_camera")
-                if rgb_img is not None:
-                    right_rgb_value.set((ts, right_rgb_count, rgb_img))
-                    right_rgb_count = (right_rgb_count + 1) % self._max_seq_num
+                if self.__right_rgb:
+                    ts, rgb_img = self.__get_rgb("right_camera")
+                    if rgb_img is not None:
+                        right_rgb_value.set((ts, right_rgb_count, rgb_img))
+                        right_rgb_count = (right_rgb_count +
+                                           1) % self._max_seq_num
                 # right depth
-                ts, depth_img = self.__get_depth("right_camera")
-                if depth_img is not None:
-                    right_depth_value.set((ts, right_depth_count, depth_img))
-                    right_depth_count = (right_depth_count +
-                                         1) % self._max_seq_num
+                if self.__right_depth:
+                    ts, depth_img = self.__get_depth("right_camera")
+                    if depth_img is not None:
+                        right_depth_value.set(
+                            (ts, right_depth_count, depth_img))
+                        right_depth_count = (right_depth_count +
+                                             1) % self._max_seq_num
             # mujoco step
             mujoco.mj_step(self.__model, self.__data)
@@ -369,8 +425,10 @@ class HexMujocoE3Desktop(HexMujocoBase):
         if not self._working.is_set():
             return
         self._working.clear()
-        self.__rgb_cam.close()
-        self.__depth_cam.close()
+        if self.__rgb_cam is not None:
+            self.__rgb_cam.close()
+        if self.__depth_cam is not None:
+            self.__depth_cam.close()
         if not self.__headless:
             self.__viewer.close()
         hex_log(HEX_LOG_LEVEL["info"], "HexMujocoE3Desktop closed")

hex_zmq_servers/zmq_base.py CHANGED Viewed

@@ -171,9 +171,12 @@ class HexZMQClientBase(ABC):
                 self.__make_socket()
             return resp_hdr, resp_buf
-    def is_working(self):
+    def is_working(self) -> bool:
         working_hdr, _ = self.request({"cmd": "is_working"})
-        return working_hdr
+        if working_hdr is None:
+            return False
+        else:
+            return working_hdr["cmd"] == "is_working_ok"
     def __send_req(self, req_dict: dict, req_buf: np.ndarray | None = None):
         # construct send header

{hex_zmq_servers-0.3.2.dist-info → hex_zmq_servers-0.3.3.dist-info}/METADATA RENAMED Viewed

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: hex_zmq_servers
-Version: 0.3.2
+Version: 0.3.3
 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>
@@ -26,16 +26,17 @@ Requires-Dist: pyzmq>=27.0.1
 Requires-Dist: hex_device<1.4.0,>=1.3.1
 Requires-Dist: hex_robo_utils<0.3.0,>=0.2.0
 Requires-Dist: dynamixel-sdk==3.8.4
-Provides-Extra: camera
-Requires-Dist: berxel_py_wrapper>=2.0.182; extra == "camera"
-Requires-Dist: opencv-python>=4.2; extra == "camera"
+Requires-Dist: opencv-python>=4.2
+Provides-Extra: berxel
+Requires-Dist: berxel_py_wrapper>=2.0.182; extra == "berxel"
+Provides-Extra: realsense
+Requires-Dist: pyrealsense2>=2.56.5.9235; extra == "realsense"
 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.182; extra == "all"
 Requires-Dist: mujoco>=3.3.3; extra == "all"
-Requires-Dist: opencv-python>=4.2; extra == "all"
+Requires-Dist: pyrealsense2>=2.56.5.9235; extra == "all"
 Dynamic: license-file
 # hex_zmq_servers

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

hex-zmq-servers 0.3.2py3-none-any.whl → 0.3.3py3-none-any.whl