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

hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6_srv.py

@@ -0,0 +1,148 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-16
+################################################################
+
+import numpy as np
+from collections import deque
+
+try:
+    from ..mujoco_base import HexMujocoServerBase
+    from .mujoco_archer_y6 import HexMujocoArcherY6
+except (ImportError, ValueError):
+    import sys
+    from pathlib import Path
+    this_file = Path(__file__).resolve()
+    project_root = this_file.parents[3]
+    if str(project_root) not in sys.path:
+        sys.path.insert(0, str(project_root))
+    from hex_zmq_servers.mujoco.mujoco_base import HexMujocoServerBase
+    from hex_zmq_servers.mujoco.archer_y6.mujoco_archer_y6 import HexMujocoArcherY6
+
+NET_CONFIG = {
+    "ip": "127.0.0.1",
+    "port": 12345,
+    "realtime_mode": False,
+    "deque_maxlen": 10,
+    "client_timeout_ms": 200,
+    "server_timeout_ms": 1_000,
+    "server_num_workers": 4,
+}
+
+MUJOCO_CONFIG = {
+    "states_rate": 1000,
+    "img_rate": 30,
+    "headless": False,
+    "sens_ts": True,
+}
+
+
+class HexMujocoArcherY6Server(HexMujocoServerBase):
+
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+        params_config: dict = MUJOCO_CONFIG,
+    ):
+        HexMujocoServerBase.__init__(self, net_config)
+
+        # mujoco
+        self._device = HexMujocoArcherY6(
+            params_config, net_config.get("realtime_mode", False))
+
+        # values
+        self._states_robot_queue = deque(maxlen=self._deque_maxlen)
+        self._states_obj_queue = deque(maxlen=self._deque_maxlen)
+        self._rgb_queue = deque(maxlen=self._deque_maxlen)
+        self._depth_queue = deque(maxlen=self._deque_maxlen)
+
+    def work_loop(self):
+        try:
+            self._device.work_loop([
+                self._states_robot_queue,
+                self._states_obj_queue,
+                self._cmds_queue,
+                self._rgb_queue,
+                self._depth_queue,
+                self._stop_event,
+            ])
+        finally:
+            self._device.close()
+
+    def _get_states(self, recv_hdr: dict):
+        try:
+            seq = recv_hdr["args"]
+        except KeyError:
+            print(f"\033[91m{recv_hdr['cmd']} requires `args`\033[0m")
+            return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
+
+        # get robot name
+        robot_name = recv_hdr["cmd"].split("_")[2]
+        if robot_name == "robot":
+            queue = self._states_robot_queue
+        elif robot_name == "obj":
+            queue = self._states_obj_queue
+        else:
+            raise ValueError(
+                f"unknown robot name: {robot_name} in {recv_hdr['cmd']}")
+
+        try:
+            ts, count, states = queue[
+                -1] if self._realtime_mode else 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
+
+        delta = (count - seq) % self._max_seq_num
+        if delta >= 0 and delta < 1e6:
+            return {
+                "cmd": f"{recv_hdr['cmd']}_ok",
+                "ts": ts,
+                "args": count
+            }, states
+        else:
+            return {"cmd": f"{recv_hdr['cmd']}_failed"}, None
+
+    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":
+            dofs = self._device.get_dofs()
+            return self.no_ts_hdr(recv_hdr, dofs is not None), dofs
+        elif recv_hdr["cmd"] == "get_limits":
+            limits = self._device.get_limits()
+            return self.no_ts_hdr(recv_hdr, limits is not None), limits
+        elif (recv_hdr["cmd"] == "get_states_robot") or (recv_hdr["cmd"]
+                                                         == "get_states_obj"):
+            return self._get_states(recv_hdr)
+        elif recv_hdr["cmd"] == "set_cmds":
+            return self._set_cmds(recv_hdr, recv_buf)
+        elif recv_hdr["cmd"] == "get_intri":
+            intri = self._device.get_intri()
+            return self.no_ts_hdr(recv_hdr, intri is not None), intri
+        elif (recv_hdr["cmd"] == "get_rgb") or (recv_hdr["cmd"]
+                                                == "get_depth"):
+            return self._get_frame(recv_hdr)
+        else:
+            raise ValueError(f"unknown command: {recv_hdr['cmd']}")
+
+
+if __name__ == "__main__":
+    import argparse, json
+    from hex_zmq_servers.zmq_base import hex_server_helper
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--cfg", type=str, required=True)
+    args = parser.parse_args()
+    cfg = json.loads(args.cfg)
+
+    hex_server_helper(cfg, HexMujocoArcherY6Server)

hex_zmq_servers/mujoco/e3_desktop/__init__.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-15
+################################################################
+
+from .mujoco_e3_desktop import HexMujocoE3Desktop
+from .mujoco_e3_desktop_cli import HexMujocoE3DesktopClient
+from .mujoco_e3_desktop_srv import HexMujocoE3DesktopServer
+
+__all__ = [
+    "HexMujocoE3Desktop",
+    "HexMujocoE3DesktopClient",
+    "HexMujocoE3DesktopServer",
+]

hex_zmq_servers/mujoco/e3_desktop/model/assets/assets.xml

@@ -0,0 +1,18 @@
+<mujocoinclude>
+    <mesh name="e3_desktop_base_link" file="e3_desktop_base_link.STL"/>
+    <mesh name="arm_base_link" file="arm_base_link.STL"/>
+    <mesh name="arm_link_1" file="arm_link_1.STL"/>
+    <mesh name="arm_link_2" file="arm_link_2.STL"/>
+    <mesh name="arm_link_3" file="arm_link_3.STL"/>
+    <mesh name="arm_link_4" file="arm_link_4.STL"/>
+    <mesh name="arm_link_5" file="arm_link_5.STL"/>
+    <mesh name="gripper_base_link" file="gripper_base_link.STL"/>
+    <mesh name="gripper_left_helper_link" file="gripper_left_helper_link.STL"/>
+    <mesh name="gripper_left_link_1" file="gripper_left_link_1.STL"/>
+    <mesh name="gripper_left_link_2" file="gripper_left_link_2.STL"/>
+    <mesh name="gripper_right_helper_link" file="gripper_right_helper_link.STL"/>
+    <mesh name="gripper_right_link_1" file="gripper_right_link_1.STL"/>
+    <mesh name="gripper_right_link_2" file="gripper_right_link_2.STL"/>
+    <mesh name="camera_link" file="camera_link.STL"/>
+    <mesh name="table_link" file="table_link.STL" scale="0.001 0.001 0.001"/>
+</mujocoinclude>

hex_zmq_servers/mujoco/e3_desktop/model/robot.xml

@@ -0,0 +1,188 @@
+<mujocoinclude>
+    <body name="base_link">
+        <inertial pos="0.00137614 -0.0126343 0.0980263" quat="0.535082 0.473426 -0.464716 0.523062" mass="15.615" diaginertia="0.665833 0.618181 0.259713"/>
+        <geom type="mesh" rgba="1 1 1 1" mesh="e3_desktop_base_link" contype="0" conaffinity="0" />
+        <geom pos="0.0185 0.3 0.14502" quat="1 0 0 0" type="mesh" rgba="1 1 1 1" mesh="arm_base_link"/>
+        <geom pos="0.0185 -0.3 0.14502" quat="1 0 0 0" type="mesh" rgba="1 1 1 1" mesh="arm_base_link"/>
+        <geom pos="0.074 0 0.53575" quat="0.9238795 0.0 0.3826834 0.0" type="mesh" mesh="camera_link" rgba="0.1 0.1 0.1 1" contype="0" conaffinity="0" />
+        <camera name="head_camera" pos="0.084 0 0.52075" quat="0.6532815 0.2705981 -0.2705981 -0.6532815" fovy="70"/>
+
+        # 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.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.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.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.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.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.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"/>
+
+                                # ee frame
+                                <body name="left_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="left_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"/>
+                                    <joint name="left_gripper_left_help_joint" pos="0 0 0" axis="0 0 -1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                    <geom type="mesh" rgba="1 1 1 1" mesh="gripper_left_helper_link"/>
+                                </body>
+                                <body name="left_gripper_right_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"/>
+                                    <joint name="left_gripper_right_help_joint" pos="0 0 0" axis="0 0 1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                    <geom type="mesh" rgba="1 1 1 1" mesh="gripper_right_helper_link"/>
+                                </body>
+
+                                # left gripper
+                                <body name="left_gripper_left_link_1" pos="0.0721 0.014 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"/>
+                                    <joint name="left_gripper_left_joint_1" pos="0 0 0" axis="0 0 -1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                    <geom type="mesh" rgba="1 1 1 1" mesh="gripper_left_link_1"/>
+
+                                    <body name="left_gripper_left_link_2" pos="0.0085108 0.04927 0">
+                                        <inertial pos="0.0255016 0.00914642 -0.00175617" quat="0.424541 0.412629 0.571518 0.568216" mass="0.0339319" diaginertia="1.36949e-05 1.35554e-05 3.4264e-06"/>
+                                        <joint name="left_gripper_left_joint_2" pos="0 0 0" axis="0 0 1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                        <geom type="mesh" rgba="1 1 1 1" mesh="gripper_left_link_2"/>
+                                    </body>
+                                </body>
+
+                                # right gripper
+                                <body name="left_gripper_right_link_1" pos="0.0721 -0.014 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"/>
+                                    <joint name="left_gripper_right_joint_1" pos="0 0 0" axis="0 0 1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                    <geom type="mesh" rgba="1 1 1 1" mesh="gripper_right_link_1"/>
+
+                                    <body name="left_gripper_right_link_2" pos="0.0085108 -0.04927 0">
+                                        <inertial pos="0.0255016 -0.00914642 -0.00175616" quat="0.568212 0.571522 0.412623 0.424547" mass="0.0339319" diaginertia="1.36948e-05 1.35553e-05 3.4264e-06"/>
+                                        <joint name="left_gripper_right_joint_2" pos="0 0 0" axis="0 0 -1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                        <geom type="mesh" rgba="1 1 1 1" mesh="gripper_right_link_2"/>
+                                    </body>
+                                </body>
+                            </body>
+                        </body>
+                    </body>
+                </body>
+            </body>
+        </body>
+
+        # 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.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.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.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.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.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.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"/>
+
+                                # ee frame
+                                <body name="right_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="right_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"/>
+                                    <joint name="right_gripper_left_help_joint" pos="0 0 0" axis="0 0 -1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                    <geom type="mesh" rgba="1 1 1 1" mesh="gripper_left_helper_link"/>
+                                </body>
+                                <body name="right_gripper_right_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"/>
+                                    <joint name="right_gripper_right_help_joint" pos="0 0 0" axis="0 0 1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                    <geom type="mesh" rgba="1 1 1 1" mesh="gripper_right_helper_link"/>
+                                </body>
+
+                                # left gripper
+                                <body name="right_gripper_left_link_1" pos="0.0721 0.014 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"/>
+                                    <joint name="right_gripper_left_joint_1" pos="0 0 0" axis="0 0 -1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                    <geom type="mesh" rgba="1 1 1 1" mesh="gripper_left_link_1"/>
+
+                                    <body name="right_gripper_left_link_2" pos="0.0085108 0.04927 0">
+                                        <inertial pos="0.0255016 0.00914642 -0.00175617" quat="0.424541 0.412629 0.571518 0.568216" mass="0.0339319" diaginertia="1.36949e-05 1.35554e-05 3.4264e-06"/>
+                                        <joint name="right_gripper_left_joint_2" pos="0 0 0" axis="0 0 1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                        <geom type="mesh" rgba="1 1 1 1" mesh="gripper_left_link_2"/>
+                                    </body>
+                                </body>
+
+                                # right gripper
+                                <body name="right_gripper_right_link_1" pos="0.0721 -0.014 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"/>
+                                    <joint name="right_gripper_right_joint_1" pos="0 0 0" axis="0 0 1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                    <geom type="mesh" rgba="1 1 1 1" mesh="gripper_right_link_1"/>
+
+                                    <body name="right_gripper_right_link_2" pos="0.0085108 -0.04927 0">
+                                        <inertial pos="0.0255016 -0.00914642 -0.00175616" quat="0.568212 0.571522 0.412623 0.424547" mass="0.0339319" diaginertia="1.36948e-05 1.35553e-05 3.4264e-06"/>
+                                        <joint name="right_gripper_right_joint_2" pos="0 0 0" axis="0 0 -1" range="0 1.52" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
+                                        <geom type="mesh" rgba="1 1 1 1" mesh="gripper_right_link_2"/>
+                                    </body>
+                                </body>
+                            </body>
+                        </body>
+                    </body>
+                </body>
+            </body>
+        </body>
+    </body>
+</mujocoinclude>

hex_zmq_servers/mujoco/e3_desktop/model/scene.xml

@@ -0,0 +1,53 @@
+<mujoco model="e3_desktop scene">
+    <compiler angle="radian"/>
+    <option integrator="implicitfast"/>
+    <option timestep="1e-3"/>
+
+    <statistic center="0.3 0 0.45" extent="0.8" meansize="0.05"/>
+
+    <visual>
+        <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0"/>
+        <rgba haze="0.15 0.25 0.35 1"/>
+        <global azimuth="120" elevation="-20"/>
+        <map znear="0.01" zfar="50"/>
+    </visual>
+
+    <asset>
+        <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
+        <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300"/>
+        <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2"/>
+        <include file="assets/assets.xml"/>
+    </asset>
+
+    <worldbody>
+        <light pos="0.0 0.0 1.5" directional="true"/>
+        <geom name="floor" size="0.0 0.0 0.05" type="plane" material="groundplane"/>
+
+        <body pos="0.0 0.0 0.15">
+            <include file="robot.xml"/>
+        </body>
+
+        <body name="table" pos="0.5 0.0 0.3" quat="0.707107 0 0 0.707107">
+            <geom mesh="table_link" pos="0 0 0" type="mesh" name="table" rgba="0.2 0.2 0.2 1" />
+        </body>
+        <body name="box" pos="0.5 0.0 0.5">
+            <joint name="red_box_joint" type="free" frictionloss="0.01" />
+            <inertial pos="0 0 0" mass="0.05" diaginertia="0.002 0.002 0.002" />
+            <geom condim="4" solimp="2 1 0.01" solref="0.01 1" friction="1 0.005 0.0001" pos="0 0 0" size="0.02 0.02 0.02" type="box" name="red_box" rgba="1 0 0 1" />
+        </body>
+        <geom name="tar_plane" pos="0.3 0.1 0.301" type="box" size="0.04 0.04 0.001" rgba="0 1 0 1" />
+    </worldbody>
+
+    <include file="setting.xml"/>
+
+    <keyframe>
+        # qpos: 12 * 2 + 7
+        # ctrl: 7 * 2
+        <key name="home" qpos="
+        0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0
+        0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0
+        0.5 0.0 0.5 1.0 0.0 0.0 0.0" ctrl="
+        0.0 0.0 0.0 0.0 0.0 0.0 0.0
+        0.0 0.0 0.0 0.0 0.0 0.0 0.0"/>
+    </keyframe>
+</mujoco>

hex_zmq_servers/mujoco/e3_desktop/model/setting.xml

@@ -0,0 +1,72 @@
+<mujocoinclude>
+    <contact>
+        # left arm
+        <exclude body1="base_link" body2="left_link_1" />
+        <exclude body1="left_link_1" body2="left_link_2" />
+        <exclude body1="left_link_2" body2="left_link_3" />
+        <exclude body1="left_link_3" body2="left_link_4" />
+        <exclude body1="left_link_4" body2="left_link_5" />
+        <exclude body1="left_link_5" body2="left_gripper_base_link" />
+        <exclude body1="left_gripper_base_link" body2="left_gripper_left_helper_link" />
+        <exclude body1="left_gripper_left_helper_link" body2="left_gripper_left_link_1" />
+        <exclude body1="left_gripper_left_helper_link" body2="left_gripper_left_link_2" />
+        <exclude body1="left_gripper_left_link_1" body2="left_gripper_left_link_2" />
+        <exclude body1="left_gripper_base_link" body2="left_gripper_right_helper_link" />
+        <exclude body1="left_gripper_right_helper_link" body2="left_gripper_right_link_1" />
+        <exclude body1="left_gripper_right_helper_link" body2="left_gripper_right_link_2" />
+        <exclude body1="left_gripper_right_link_1" body2="left_gripper_right_link_2" />
+
+        # right arm
+        <exclude body1="base_link" body2="right_link_1" />
+        <exclude body1="right_link_1" body2="right_link_2" />
+        <exclude body1="right_link_2" body2="right_link_3" />
+        <exclude body1="right_link_3" body2="right_link_4" />
+        <exclude body1="right_link_4" body2="right_link_5" />
+        <exclude body1="right_link_5" body2="right_gripper_base_link" />
+        <exclude body1="right_gripper_base_link" body2="right_gripper_left_helper_link" />
+        <exclude body1="right_gripper_left_helper_link" body2="right_gripper_left_link_1" />
+        <exclude body1="right_gripper_left_helper_link" body2="right_gripper_left_link_2" />
+        <exclude body1="right_gripper_left_link_1" body2="right_gripper_left_link_2" />
+        <exclude body1="right_gripper_base_link" body2="right_gripper_right_helper_link" />
+        <exclude body1="right_gripper_right_helper_link" body2="right_gripper_right_link_1" />
+        <exclude body1="right_gripper_right_helper_link" body2="right_gripper_right_link_2" />
+        <exclude body1="right_gripper_right_link_1" body2="right_gripper_right_link_2" />
+    </contact>
+
+    <equality>
+        # left arm
+        <joint joint1="left_gripper_left_joint_1" joint2="left_gripper_left_joint_2" />
+        <joint joint1="left_gripper_left_joint_1" joint2="left_gripper_left_help_joint" />
+        <joint joint1="left_gripper_left_joint_1" joint2="left_gripper_right_joint_1" />
+        <joint joint1="left_gripper_left_joint_1" joint2="left_gripper_right_joint_2" />
+        <joint joint1="left_gripper_left_joint_1" joint2="left_gripper_right_help_joint" />
+
+        # right arm
+        <joint joint1="right_gripper_left_joint_1" joint2="right_gripper_left_joint_2" />
+        <joint joint1="right_gripper_left_joint_1" joint2="right_gripper_left_help_joint" />
+        <joint joint1="right_gripper_left_joint_1" joint2="right_gripper_right_joint_1" />
+        <joint joint1="right_gripper_left_joint_1" joint2="right_gripper_right_joint_2" />
+        <joint joint1="right_gripper_left_joint_1" joint2="right_gripper_right_help_joint" />
+    </equality>
+
+    <actuator>
+        # left arm
+        <motor joint="left_joint_1" ctrlrange="-28 28" forcerange="-28 28"/>
+        <motor joint="left_joint_2" ctrlrange="-28 28" forcerange="-28 28"/>
+        <motor joint="left_joint_3" ctrlrange="-28 28" forcerange="-28 28"/>
+        <motor joint="left_joint_4" ctrlrange="-10 10" forcerange="-10 10"/>
+        <motor joint="left_joint_5" ctrlrange="-10 10" forcerange="-10 10"/>
+        <motor joint="left_joint_6" ctrlrange="-10 10" forcerange="-10 10"/>
+        <motor joint="left_gripper_left_joint_1" ctrlrange="-10 10" forcerange="-10 10"/>
+
+        # right arm
+        <motor joint="right_joint_1" ctrlrange="-28 28" forcerange="-28 28"/>
+        <motor joint="right_joint_2" ctrlrange="-28 28" forcerange="-28 28"/>
+        <motor joint="right_joint_3" ctrlrange="-28 28" forcerange="-28 28"/>
+        <motor joint="right_joint_4" ctrlrange="-10 10" forcerange="-10 10"/>
+        <motor joint="right_joint_5" ctrlrange="-10 10" forcerange="-10 10"/>
+        <motor joint="right_joint_6" ctrlrange="-10 10" forcerange="-10 10"/>
+        <motor joint="right_gripper_left_joint_1" ctrlrange="-10 10" forcerange="-10 10"/>
+    </actuator>
+
+</mujocoinclude>