PyPI - hex-zmq-servers - Versions diffs - 0.3.9__py3-none-any.whl - Mend

hex-zmq-servers 0.3.9__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 (110) hide show

hex_zmq_servers/__init__.py +173 -0
hex_zmq_servers/cam/__init__.py +52 -0
hex_zmq_servers/cam/berxel/__init__.py +17 -0
hex_zmq_servers/cam/berxel/cam_berxel.py +282 -0
hex_zmq_servers/cam/berxel/cam_berxel_cli.py +33 -0
hex_zmq_servers/cam/berxel/cam_berxel_srv.py +79 -0
hex_zmq_servers/cam/cam_base.py +189 -0
hex_zmq_servers/cam/dummy/__init__.py +17 -0
hex_zmq_servers/cam/dummy/cam_dummy.py +69 -0
hex_zmq_servers/cam/dummy/cam_dummy_cli.py +29 -0
hex_zmq_servers/cam/dummy/cam_dummy_srv.py +68 -0
hex_zmq_servers/cam/realsense/__init__.py +17 -0
hex_zmq_servers/cam/realsense/cam_realsense.py +159 -0
hex_zmq_servers/cam/realsense/cam_realsense_cli.py +33 -0
hex_zmq_servers/cam/realsense/cam_realsense_srv.py +78 -0
hex_zmq_servers/cam/rgb/__init__.py +17 -0
hex_zmq_servers/cam/rgb/cam_rgb.py +135 -0
hex_zmq_servers/cam/rgb/cam_rgb_cli.py +43 -0
hex_zmq_servers/cam/rgb/cam_rgb_srv.py +78 -0
hex_zmq_servers/config/cam_berxel.json +18 -0
hex_zmq_servers/config/cam_dummy.json +12 -0
hex_zmq_servers/config/cam_realsense.json +17 -0
hex_zmq_servers/config/cam_rgb.json +28 -0
hex_zmq_servers/config/mujoco_archer_y6.json +37 -0
hex_zmq_servers/config/mujoco_e3_desktop.json +41 -0
hex_zmq_servers/config/robot_dummy.json +153 -0
hex_zmq_servers/config/robot_gello.json +66 -0
hex_zmq_servers/config/robot_hexarm.json +37 -0
hex_zmq_servers/config/zmq_dummy.json +12 -0
hex_zmq_servers/device_base.py +44 -0
hex_zmq_servers/hex_launch.py +489 -0
hex_zmq_servers/mujoco/__init__.py +28 -0
hex_zmq_servers/mujoco/archer_y6/__init__.py +17 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/arm_base_link.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/arm_link_1.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/arm_link_2.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/arm_link_3.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/arm_link_4.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/arm_link_5.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/assets.xml +17 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/camera_link.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/gripper_base_link.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/gripper_left_helper_link.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/gripper_left_link_1.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/gripper_left_link_2.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/gripper_right_helper_link.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/gripper_right_link_1.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/gripper_right_link_2.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/assets/table_link.STL +0 -0
hex_zmq_servers/mujoco/archer_y6/model/robot.xml +95 -0
hex_zmq_servers/mujoco/archer_y6/model/scene.xml +51 -0
hex_zmq_servers/mujoco/archer_y6/model/setting.xml +37 -0
hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6.py +325 -0
hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6_cli.py +71 -0
hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6_srv.py +148 -0
hex_zmq_servers/mujoco/e3_desktop/__init__.py +17 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/arm_base_link.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/arm_link_1.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/arm_link_2.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/arm_link_3.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/arm_link_4.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/arm_link_5.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/assets.xml +18 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/camera_link.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/e3_desktop_base_link.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/gripper_base_link.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/gripper_left_helper_link.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/gripper_left_link_1.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/gripper_left_link_2.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/gripper_right_helper_link.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/gripper_right_link_1.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/gripper_right_link_2.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/assets/table_link.STL +0 -0
hex_zmq_servers/mujoco/e3_desktop/model/robot.xml +188 -0
hex_zmq_servers/mujoco/e3_desktop/model/scene.xml +53 -0
hex_zmq_servers/mujoco/e3_desktop/model/setting.xml +72 -0
hex_zmq_servers/mujoco/e3_desktop/mujoco_e3_desktop.py +449 -0
hex_zmq_servers/mujoco/e3_desktop/mujoco_e3_desktop_cli.py +289 -0
hex_zmq_servers/mujoco/e3_desktop/mujoco_e3_desktop_srv.py +244 -0
hex_zmq_servers/mujoco/mujoco_base.py +425 -0
hex_zmq_servers/robot/__init__.py +37 -0
hex_zmq_servers/robot/dummy/__init__.py +17 -0
hex_zmq_servers/robot/dummy/robot_dummy.py +94 -0
hex_zmq_servers/robot/dummy/robot_dummy_cli.py +29 -0
hex_zmq_servers/robot/dummy/robot_dummy_srv.py +82 -0
hex_zmq_servers/robot/gello/__init__.py +17 -0
hex_zmq_servers/robot/gello/robot_gello.py +366 -0
hex_zmq_servers/robot/gello/robot_gello_cli.py +29 -0
hex_zmq_servers/robot/gello/robot_gello_srv.py +93 -0
hex_zmq_servers/robot/hexarm/__init__.py +47 -0
hex_zmq_servers/robot/hexarm/robot_hexarm.py +292 -0
hex_zmq_servers/robot/hexarm/robot_hexarm_cli.py +37 -0
hex_zmq_servers/robot/hexarm/robot_hexarm_srv.py +87 -0
hex_zmq_servers/robot/hexarm/urdf/archer_l6y/empty.urdf +206 -0
hex_zmq_servers/robot/hexarm/urdf/archer_l6y/gp100.urdf +206 -0
hex_zmq_servers/robot/hexarm/urdf/archer_l6y/gp100_handle.urdf +206 -0
hex_zmq_servers/robot/hexarm/urdf/archer_l6y/gp100_p050.urdf +206 -0
hex_zmq_servers/robot/hexarm/urdf/archer_l6y/gp100_p050_handle.urdf +206 -0
hex_zmq_servers/robot/hexarm/urdf/archer_y6/empty.urdf +207 -0
hex_zmq_servers/robot/hexarm/urdf/archer_y6/gp100.urdf +207 -0
hex_zmq_servers/robot/hexarm/urdf/archer_y6/gp100_handle.urdf +207 -0
hex_zmq_servers/robot/hexarm/urdf/archer_y6/gp100_p050.urdf +207 -0
hex_zmq_servers/robot/hexarm/urdf/archer_y6/gp100_p050_handle.urdf +207 -0
hex_zmq_servers/robot/robot_base.py +276 -0
hex_zmq_servers/zmq_base.py +547 -0
hex_zmq_servers-0.3.9.dist-info/METADATA +147 -0
hex_zmq_servers-0.3.9.dist-info/RECORD +110 -0
hex_zmq_servers-0.3.9.dist-info/WHEEL +5 -0
hex_zmq_servers-0.3.9.dist-info/licenses/LICENSE +201 -0
hex_zmq_servers-0.3.9.dist-info/top_level.txt +1 -0

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

@@ -0,0 +1,95 @@
+<mujocoinclude>
+    <body name="base_link">
+        <inertial pos="-0.001 0.0318 0" quat="0.5 0.5 0.5 0.5" mass="0.687" diaginertia="0.0007 0.0006 0.0005"/>
+        <geom type="mesh" rgba="1 1 1 1" mesh="arm_base_link"/>
+        # 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.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.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.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.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.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.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"/>
+                                # ee frame
+                                <body name="ee_frame" pos="0.1870 0.0 0.0" quat="1 0 0 0">
+                                    <geom type="cylinder" size="0.002 0.01" pos="0.01 0.00 0.00" quat="0.7071068 0.0000000 0.7071068 0.0000000" rgba="1 0 0 1" contype="0" conaffinity="0" group="4"/>
+                                    <geom type="cylinder" size="0.002 0.01" pos="0.00 0.01 0.00" quat="0.7071068 0.7071068 0.0000000 0.0000000" rgba="0 1 0 1" contype="0" conaffinity="0" group="4"/>
+                                    <geom type="cylinder" size="0.002 0.01" pos="0.00 0.00 0.01" quat="1.0000000 0.0000000 0.0000000 0.0000000" rgba="0 0 1 1" contype="0" conaffinity="0" group="4"/>
+                                </body>
+                                # gripper helper
+                                <body name="gripper_left_helper_link" pos="0.057667 0.030604 2.5e-05">
+                                    <inertial pos="0.00299357 0.0232625 -5e-05" quat="0.996724 0 0 0.0808839" mass="0.00594562" diaginertia="2.74798e-06 1.47567e-06 1.34732e-06"/>
+                                    <joint name="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="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="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="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="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="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="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="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="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="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="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/archer_y6/model/scene.xml ADDED Viewed

@@ -0,0 +1,51 @@
+<mujoco model="archer_y6 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.001" 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.25">
+            <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.6 0.0 0.301" type="box" size="0.04 0.04 0.001" rgba="0 1 0 1" />
+    </worldbody>
+    <include file="setting.xml"/>
+    <keyframe>
+        # qpos: 12 + 7
+        # ctrl: 7
+        <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.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"/>
+    </keyframe>
+</mujoco>

hex_zmq_servers/mujoco/archer_y6/model/setting.xml ADDED Viewed

@@ -0,0 +1,37 @@
+<mujocoinclude>
+    <contact>
+        <exclude body1="base_link" body2="link_1" />
+        <exclude body1="link_1" body2="link_2" />
+        <exclude body1="link_2" body2="link_3" />
+        <exclude body1="link_3" body2="link_4" />
+        <exclude body1="link_4" body2="link_5" />
+        <exclude body1="link_5" body2="gripper_base_link" />
+        <exclude body1="gripper_base_link" body2="gripper_left_helper_link" />
+        <exclude body1="gripper_left_helper_link" body2="gripper_left_link_1" />
+        <exclude body1="gripper_left_helper_link" body2="gripper_left_link_2" />
+        <exclude body1="gripper_left_link_1" body2="gripper_left_link_2" />
+        <exclude body1="gripper_base_link" body2="gripper_right_helper_link" />
+        <exclude body1="gripper_right_helper_link" body2="gripper_right_link_1" />
+        <exclude body1="gripper_right_helper_link" body2="gripper_right_link_2" />
+        <exclude body1="gripper_right_link_1" body2="gripper_right_link_2" />
+    </contact>
+    <equality>
+        <joint joint1="gripper_left_joint_1" joint2="gripper_left_joint_2" />
+        <joint joint1="gripper_left_joint_1" joint2="gripper_left_help_joint" />
+        <joint joint1="gripper_left_joint_1" joint2="gripper_right_joint_1" />
+        <joint joint1="gripper_left_joint_1" joint2="gripper_right_joint_2" />
+        <joint joint1="gripper_left_joint_1" joint2="gripper_right_help_joint" />
+    </equality>
+    <actuator>
+        <motor joint="joint_1" ctrlrange="-28 28" forcerange="-28 28"/>
+        <motor joint="joint_2" ctrlrange="-28 28" forcerange="-28 28"/>
+        <motor joint="joint_3" ctrlrange="-28 28" forcerange="-28 28"/>
+        <motor joint="joint_4" ctrlrange="-10 10" forcerange="-10 10"/>
+        <motor joint="joint_5" ctrlrange="-10 10" forcerange="-10 10"/>
+        <motor joint="joint_6" ctrlrange="-10 10" forcerange="-10 10"/>
+        <motor joint="gripper_left_joint_1" ctrlrange="-10 10" forcerange="-10 10"/>
+    </actuator>
+</mujocoinclude>

hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6.py ADDED Viewed

@@ -0,0 +1,325 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-17
+################################################################
+import os
+import copy
+import threading
+import cv2
+import numpy as np
+from collections import deque
+import mujoco
+from mujoco import viewer
+from ..mujoco_base import HexMujocoBase
+from ...zmq_base import (
+    hex_ns_now,
+    hex_zmq_ts_now,
+    ns_to_hex_zmq_ts,
+    hex_zmq_ts_delta_ms,
+    HexRate,
+)
+from ...hex_launch import hex_log, HEX_LOG_LEVEL
+from hex_robo_utils import HexCtrlUtilMitJoint as CtrlUtil
+MUJOCO_CONFIG = {
+    "states_rate": 1000,
+    "img_rate": 30,
+    "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],
+    "headless": False,
+    "sens_ts": True,
+}
+CAMERA_CONFIG = {
+    "empty": (False, False),
+    "rgb": (True, False),
+    "berxel": (True, True),
+    "realsense": (True, True),
+}
+class HexMujocoArcherY6(HexMujocoBase):
+    def __init__(
+        self,
+        mujoco_config: dict = MUJOCO_CONFIG,
+        realtime_mode: bool = False,
+    ):
+        HexMujocoBase.__init__(self, realtime_mode)
+        try:
+            states_rate = mujoco_config["states_rate"]
+            img_rate = mujoco_config["img_rate"]
+            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:
+            missing_key = ke.args[0]
+            raise ValueError(
+                f"mujoco_config is not valid, missing key: {missing_key}")
+        # mujoco init
+        model_path = os.path.join(os.path.dirname(__file__), "model/scene.xml")
+        self.__model = mujoco.MjModel.from_xml_path(model_path)
+        self.__data = mujoco.MjData(self.__model)
+        self.__sim_rate = int(1.0 / self.__model.opt.timestep)
+        # state init
+        self.__state_robot_idx = [0, 1, 2, 3, 4, 5, 6]
+        self.__state_obj_idx = [12, 13, 14, 15, 16, 17, 18]
+        self.__ctrl_robot_idx = [0, 1, 2, 3, 4, 5, 6]
+        self.__ctrl_obj_idx = [0, 1, 2, 3, 4, 5, 6]
+        self._limits = np.stack(
+            [self.__model.jnt_range[self.__state_robot_idx, :]],
+            axis=0,
+        )
+        if not self.__tau_ctrl:
+            self.__mit_kp = np.ascontiguousarray(np.asarray(self.__mit_kp))
+            self.__mit_kd = np.ascontiguousarray(np.asarray(self.__mit_kd))
+            self.__mit_ctrl = CtrlUtil()
+        self.__gripper_ratio = 1.33 / 1.52
+        self._limits[0, -1] *= self.__gripper_ratio
+        self._dofs = np.array([len(self.__state_robot_idx)])
+        keyframe_id = mujoco.mj_name2id(
+            self.__model,
+            mujoco.mjtObj.mjOBJ_KEY,
+            "home",
+        )
+        self.__state_init = {
+            "qpos": self.__model.key_qpos[keyframe_id],
+            "qvel": np.zeros_like(self.__data.qvel),
+            "ctrl": np.zeros_like(self.__data.ctrl),
+        }
+        self.__data.qpos = self.__state_init["qpos"]
+        self.__data.qvel = self.__state_init["qvel"]
+        self.__data.ctrl = self.__state_init["ctrl"]
+        self.__states_trig_thresh = int(self.__sim_rate / states_rate)
+        # camera init
+        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)
+        if not self.__headless:
+            self.__viewer = viewer.launch_passive(self.__model, self.__data)
+        # start work loop
+        self._working.set()
+    def __del__(self):
+        HexMujocoBase.__del__(self)
+    def reset(self) -> bool:
+        self.__data.qpos = self.__state_init["qpos"]
+        self.__data.qvel = self.__state_init["qvel"]
+        self.__data.ctrl = self.__state_init["ctrl"]
+        mujoco.mj_forward(self.__model, self.__data)
+        if not self.__headless:
+            self.__viewer.sync()
+        return True
+    def work_loop(self, hex_queues: list[deque | threading.Event]):
+        states_robot_queue = hex_queues[0]
+        states_obj_queue = hex_queues[1]
+        cmds_robot_queue = hex_queues[2]
+        rgb_queue = hex_queues[3]
+        depth_queue = hex_queues[4]
+        stop_event = hex_queues[5]
+        last_states_ts = {"s": 0, "ns": 0}
+        states_robot_count = 0
+        states_obj_count = 0
+        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
+        self.__bias_ns = hex_ns_now() - self.__data.time * 1_000_000_000
+        init_ts = self.__mujoco_ts() if self.__sens_ts else hex_zmq_ts_now()
+        rgb_queue.append((init_ts, 0,
+                          np.zeros((self.__height, self.__width, 3),
+                                   dtype=np.uint8)))
+        depth_queue.append((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:
+                states_trig_count = 0
+                # states
+                ts, states_robot, states_obj = self.__get_states()
+                if states_robot is not None:
+                    if hex_zmq_ts_delta_ms(ts, last_states_ts) > 1e-6:
+                        last_states_ts = ts
+                        # states robot
+                        states_robot_queue.append(
+                            (ts, states_robot_count, states_robot))
+                        states_robot_count = (states_robot_count +
+                                              1) % self._max_seq_num
+                        # states obj
+                        states_obj_queue.append(
+                            (ts, states_obj_count, states_obj))
+                        states_obj_count = (states_obj_count +
+                                            1) % self._max_seq_num
+                # cmds
+                cmds_robot_pack = None
+                try:
+                    cmds_robot_pack = cmds_robot_queue[
+                        -1] if self._realtime_mode else cmds_robot_queue.popleft(
+                        )
+                except IndexError:
+                    pass
+                if cmds_robot_pack is not None:
+                    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:
+                            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
+                if self.__use_rgb:
+                    ts, rgb_img = self.__get_rgb()
+                    if rgb_img is not None:
+                        rgb_queue.append((ts, rgb_count, rgb_img))
+                        rgb_count = (rgb_count + 1) % self._max_seq_num
+                # depth
+                if self.__use_depth:
+                    ts, depth_img = self.__get_depth()
+                    if depth_img is not None:
+                        depth_queue.append((ts, depth_count, depth_img))
+                        depth_count = (depth_count + 1) % self._max_seq_num
+            # mujoco step
+            mujoco.mj_step(self.__model, self.__data)
+            if not self.__headless:
+                self.__viewer.sync()
+            # sleep
+            rate.sleep()
+        # close
+        self.close()
+    def __get_states(self):
+        pos = copy.deepcopy(self.__data.qpos)
+        vel = copy.deepcopy(self.__data.qvel)
+        eff = copy.deepcopy(self.__data.qfrc_actuator)
+        pos[self.__state_robot_idx[-1]] = pos[
+            self.__state_robot_idx[-1]] * self.__gripper_ratio
+        return self.__mujoco_ts() if self.__sens_ts else hex_zmq_ts_now(
+        ), np.array([
+            pos[self.__state_robot_idx],
+            vel[self.__state_robot_idx],
+            eff[self.__state_robot_idx],
+        ]).T, self.__data.qpos[self.__state_obj_idx].copy()
+    def __set_cmds(self, cmds: np.ndarray):
+        tau_cmds = None
+        if not self.__tau_ctrl:
+            cmd_pos = None
+            tar_vel = np.zeros(cmds.shape[0])
+            cmd_tor = np.zeros(cmds.shape[0])
+            cmd_kp = self.__mit_kp.copy()
+            cmd_kd = self.__mit_kd.copy()
+            if len(cmds.shape) == 1:
+                cmd_pos = cmds
+            elif len(cmds.shape) == 2:
+                if cmds.shape[1] == 2:
+                    cmd_pos = cmds[:, 0].copy()
+                    cmd_tor = cmds[:, 1].copy()
+                elif cmds.shape[1] == 5:
+                    cmd_pos = cmds[:, 0].copy()
+                    tar_vel = cmds[:, 1].copy()
+                    cmd_tor = cmds[:, 2].copy()
+                    cmd_kp = cmds[:, 3].copy()
+                    cmd_kd = cmds[:, 4].copy()
+                else:
+                    raise ValueError(
+                        f"The shape of cmds is invalid: {cmds.shape}")
+            else:
+                raise ValueError(f"The shape of cmds is invalid: {cmds.shape}")
+            tar_pos = self._apply_pos_limits(
+                cmd_pos,
+                self._limits[0, :, 0],
+                self._limits[0, :, 1],
+            )
+            tar_pos[-1] /= self.__gripper_ratio
+            tau_cmds = self.__mit_ctrl(
+                cmd_kp,
+                cmd_kd,
+                tar_pos,
+                tar_vel,
+                self.__data.qpos[self.__state_robot_idx],
+                self.__data.qvel[self.__state_robot_idx],
+                cmd_tor,
+            )
+        else:
+            tau_cmds = cmds.copy()
+        self.__data.ctrl[self.__ctrl_robot_idx] = tau_cmds
+    def __get_rgb(self):
+        self.__rgb_cam.update_scene(self.__data, "end_camera")
+        rgb_img = self.__rgb_cam.render()
+        return self.__mujoco_ts() if self.__sens_ts else hex_zmq_ts_now(
+        ), cv2.cvtColor(rgb_img, cv2.COLOR_RGB2BGR)
+    def __get_depth(self):
+        self.__depth_cam.update_scene(self.__data, "end_camera")
+        depth_m = self.__depth_cam.render().astype(np.float32)
+        depth_img = np.clip(depth_m * 1000.0, 0, 65535).astype(np.uint16)
+        return self.__mujoco_ts() if self.__sens_ts else hex_zmq_ts_now(
+        ), depth_img
+    def __mujoco_ts(self):
+        mujoco_ts = self.__data.time * 1_000_000_000 + self.__bias_ns
+        return ns_to_hex_zmq_ts(mujoco_ts)
+    def close(self):
+        if not self._working.is_set():
+            return
+        self._working.clear()
+        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/archer_y6/mujoco_archer_y6_cli.py ADDED Viewed

@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+# -*- coding:utf-8 -*-
+################################################################
+# Copyright 2025 Dong Zhaorui. All rights reserved.
+# Author: Dong Zhaorui 847235539@qq.com
+# Date  : 2025-09-12
+################################################################
+from ..mujoco_base import HexMujocoClientBase
+from ...zmq_base import HexRate
+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,
+}
+RECV_CONFIG = {
+    "rgb": True,
+    "depth": True,
+    "obj": True,
+}
+class HexMujocoArcherY6Client(HexMujocoClientBase):
+    def __init__(
+        self,
+        net_config: dict = NET_CONFIG,
+        recv_config: dict = RECV_CONFIG,
+    ):
+        HexMujocoClientBase.__init__(self, net_config)
+        self.__recv_config = recv_config
+        self._wait_for_working()
+    def _recv_loop(self):
+        rate = HexRate(2000)
+        image_trig_cnt = 0
+        while self._recv_flag:
+            hdr, states = self._get_states_inner("robot")
+            if hdr is not None:
+                self._states_queue["robot"].append((hdr, states))
+            if self.__recv_config["obj"]:
+                hdr, obj_pose = self._get_states_inner("obj")
+                if hdr is not None:
+                    self._states_queue["obj"].append((hdr, obj_pose))
+            image_trig_cnt += 1
+            if image_trig_cnt >= 10:
+                image_trig_cnt = 0
+                if self.__recv_config["rgb"]:
+                    hdr, img = self._get_rgb_inner()
+                    if hdr is not None:
+                        self._rgb_queue.append((hdr, img))
+                if self.__recv_config["depth"]:
+                    hdr, img = self._get_depth_inner()
+                    if hdr is not None:
+                        self._depth_queue.append((hdr, img))
+            try:
+                cmds = self._cmds_queue[
+                    -1] if self._realtime_mode else self._cmds_queue.popleft()
+                _ = self._set_cmds_inner(cmds)
+            except IndexError:
+                pass
+            rate.sleep()