hex-zmq-servers 0.2.0__py3-none-any.whl → 0.2.2__py3-none-any.whl

hex_zmq_servers/__init__.py

@@ -15,7 +15,6 @@ from .zmq_base import HexZMQDummyClient, HexZMQDummyServer
 
 from .cam import HexCamBase, HexCamClientBase, HexCamServerBase
 from .cam import HexCamDummy, HexCamDummyClient, HexCamDummyServer
-from .cam import HexCamBerxel, HexCamBerxelClient, HexCamBerxelServer
 
 from .mujoco import HexMujocoBase, HexMujocoClientBase, HexMujocoServerBase
 from .mujoco import HexMujocoArcherD6y, HexMujocoArcherD6yClient, HexMujocoArcherD6yServer
@@ -24,7 +23,7 @@ from .mujoco import HexMujocoE3Desktop, HexMujocoE3DesktopClient, HexMujocoE3Des
 from .robot import HexRobotBase, HexRobotClientBase, HexRobotServerBase
 from .robot import HexRobotDummy, HexRobotDummyClient, HexRobotDummyServer
 from .robot import HexRobotGello, HexRobotGelloClient, HexRobotGelloServer
-from .robot import HexRobotHexarm, HexRobotHexarmClient, HexRobotHexarmServer
+from .robot import HexRobotHexarm, HexRobotHexarmClient, HexRobotHexarmServer, HEXARM_URDF_PATH_DICT
 
 import os
 
@@ -32,7 +31,6 @@ file_dir = os.path.dirname(os.path.abspath(__file__))
 HEX_ZMQ_SERVERS_PATH_DICT = {
     "zmq_dummy": f"{file_dir}/zmq_base.py",
     "cam_dummy": f"{file_dir}/cam/dummy/cam_dummy_srv.py",
-    "cam_berxel": f"{file_dir}/cam/berxel/cam_berxel_srv.py",
     "mujoco_archer_d6y":
     f"{file_dir}/mujoco/archer_d6y/mujoco_archer_d6y_srv.py",
     "mujoco_e3_desktop":
@@ -44,7 +42,6 @@ HEX_ZMQ_SERVERS_PATH_DICT = {
 HEX_ZMQ_CONFIGS_PATH_DICT = {
     "zmq_dummy": f"{file_dir}/config/zmq_dummy.json",
     "cam_dummy": f"{file_dir}/config/cam_dummy.json",
-    "cam_berxel": f"{file_dir}/config/cam_berxel.json",
     "mujoco_archer_d6y": f"{file_dir}/config/mujoco_archer_d6y.json",
     "mujoco_e3_desktop": f"{file_dir}/config/mujoco_e3_desktop.json",
     "robot_dummy": f"{file_dir}/config/robot_dummy.json",
@@ -59,6 +56,7 @@ __all__ = [
     # path
     "HEX_ZMQ_SERVERS_PATH_DICT",
     "HEX_ZMQ_CONFIGS_PATH_DICT",
+    "HEXARM_URDF_PATH_DICT",
 
     # launch
     "HexLaunch",
@@ -85,9 +83,6 @@ __all__ = [
     "HexCamDummy",
     "HexCamDummyClient",
     "HexCamDummyServer",
-    "HexCamBerxel",
-    "HexCamBerxelClient",
-    "HexCamBerxelServer",
 
     # mujoco
     "HexMujocoBase",
@@ -115,4 +110,21 @@ __all__ = [
     "HexRobotHexarmServer",
 ]
 
+# Optional: berxel
+try:
+    from .cam import HexCamBerxel, HexCamBerxelClient, HexCamBerxelServer
+    HEX_ZMQ_SERVERS_PATH_DICT[
+        "cam_berxel"] = f"{file_dir}/cam/berxel/cam_berxel_srv.py"
+    HEX_ZMQ_CONFIGS_PATH_DICT[
+        "cam_berxel"] = f"{file_dir}/config/cam_berxel.json"
+    __all__.extend([
+        "HexCamBerxel",
+        "HexCamBerxelClient",
+        "HexCamBerxelServer",
+    ])
+except ImportError:
+    # berxel_py_wrapper not installed
+    # Install with: pip install hex_zmq_servers[berxel]
+    pass
+
 # print("#### Thanks for using hex_zmq_servers :D ####")

hex_zmq_servers/cam/__init__.py

@@ -8,7 +8,6 @@
 
 from .cam_base import HexCamBase, HexCamClientBase, HexCamServerBase
 from .dummy import HexCamDummy, HexCamDummyClient, HexCamDummyServer
-from .berxel import HexCamBerxel, HexCamBerxelClient, HexCamBerxelServer
 
 __all__ = [
     # base
@@ -20,9 +19,17 @@ __all__ = [
     "HexCamDummy",
     "HexCamDummyClient",
     "HexCamDummyServer",
-
-    # berxel
-    "HexCamBerxel",
-    "HexCamBerxelClient",
-    "HexCamBerxelServer",
 ]
+
+# Optional: berxel (requires berxel_py_wrapper)
+try:
+    from .berxel import HexCamBerxel, HexCamBerxelClient, HexCamBerxelServer
+    __all__.extend([
+        "HexCamBerxel",
+        "HexCamBerxelClient",
+        "HexCamBerxelServer",
+    ])
+except ImportError:
+    # berxel_py_wrapper not installed
+    # Install with: pip install hex_zmq_servers[berxel]
+    pass

hex_zmq_servers/config/mujoco_archer_d6y.json

@@ -9,6 +9,25 @@
     "params": {
         "states_rate": 250,
         "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
     }

hex_zmq_servers/config/mujoco_e3_desktop.json

@@ -9,6 +9,25 @@
     "params": {
         "states_rate": 250,
         "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
     }

hex_zmq_servers/mujoco/archer_d6y/model/assets/assets.xml

@@ -5,15 +5,13 @@
     <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="picker_base_link" file="picker_base_link.STL"/>
-    <mesh name="picker_narrow1_link" file="picker_narrow1_link.STL"/>
-    <mesh name="picker_narrow2_link" file="picker_narrow2_link.STL"/>
-    <mesh name="picker_narrow3_link" file="picker_narrow3_link.STL"/>
-    <mesh name="picker_narrow_loop_link" file="picker_narrow_loop_link.STL"/>
-    <mesh name="picker_wide1_link" file="picker_wide1_link.STL"/>
-    <mesh name="picker_wide2_link" file="picker_wide2_link.STL"/>
-    <mesh name="picker_wide3_link" file="picker_wide3_link.STL"/>
-    <mesh name="picker_wide_loop_link" file="picker_wide_loop_link.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/archer_d6y/model/robot.xml

@@ -6,7 +6,7 @@
         # 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.61 3.14" 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.1" damping="3.0" frictionloss="1.0"/>
             <geom type="mesh" rgba="0.92549 0.92549 0.92549 1" mesh="arm_link_1"/>
 
             # link 2
@@ -33,64 +33,51 @@
                             <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"/>
                             <geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="arm_link_5"/>
 
-                            # picker base link
-                            <body name="picker_base_link" pos="0.0547 0 0.02895">
-                                <inertial pos="0.00913219 -0.00104428 0.0437714" quat="0.779177 -0.153227 -0.105966 0.598478" mass="0.416893" diaginertia="0.00112999 0.000894143 0.000426247"/>
-                                <joint name="joint_6" pos="0 0 0" axis="0 0 1" range="-1.57 1.57" limited="true" armature="0.1" damping="3.0" frictionloss="1.0"/>
-                                <geom pos="0 0 0.005" type="mesh" mesh="picker_base_link" rgba="0.75 0.75 0.75 1.0" />
-                                <camera name="end_camera" pos="-0.07 0.0 0.03" quat="0.1227878 0.6963642 -0.6963642 -0.1227878" fovy="50"/>
-                                <geom pos="-0.08 0.0 0.015" quat="0.819152 0.0 -0.5735764 0.0" type="mesh" mesh="camera_link" rgba="0.1 0.1 0.1 1" contype="0" conaffinity="0" />
+                            # gripper base link
+                            <body name="gripper_base_link" pos="0.0547 0 0.02895" quat="0.707107 0.0 -0.707107 0.0">
+                                <inertial pos="0.0141517 0.0120671 0.037083" quat="0.332786 0.772745 0.486202 0.236063" mass="0.404149" diaginertia="0.00182378 0.00177437 0.000232349"/>
+                                <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"/>
+                                <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"/>
 
-                                # narrow gripper
-                                <body name="picker_narrow1_link" pos="0 -0.0195 0.0565" quat="0.0 -0.707 0.0 -0.707">
-                                    <inertial pos="0.0094685 0.0068806 6.5437e-05" quat="0.488058 0.00022793 0.00062709 0.872811" mass="0.025428" diaginertia="2.65562e-06 1.69368e-06 1.5501e-06"/>
-                                    <joint name="hand_narrow1_joint" pos="0 0 0" axis="0 0 1" range="0.0 0.8" limited="true" armature="0.01" damping="1.0" frictionloss="0.1" />
-                                    <geom type="mesh" mesh="picker_narrow1_link" rgba="0.75 0.75 0.75 1.0" />
-
-                                    <body name="picker_narrow2_link" pos="0.030852 0.018551 0">
-                                        <inertial pos="0.0088027 -0.007035 1.6424e-05" quat="0.259339 0.661207 -0.251356 0.657547" mass="0.0040132" diaginertia="1.70091e-07 1.66561e-07 5.03584e-08"/>
-                                        <joint name="hand_narrow2_joint" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57" limited="true" armature="0.001" damping="1.0" frictionloss="0.1" springref="-0.01" stiffness="1.0" />
-                                        <geom type="mesh" mesh="picker_narrow2_link" rgba="0.75 0.75 0.75 1.0" />
-
-                                        <body name="picker_narrow3_link" pos="0.018118 -0.01574 0">
-                                            <inertial pos="0.012508 -0.0079729 9.4339e-05" quat="0.575033 0.575033 0.411506 0.411506" mass="0.018029" diaginertia="2.75843e-06 2.3252e-06 9.52273e-07"/>
-                                            <joint name="hand_narrow3_joint" pos="0 0 0" axis="0 0 -1" range="-0.5 0.5" limited="true" armature="0.001" damping="1.0" frictionloss="0.1" />
-                                            <geom type="mesh" mesh="picker_narrow3_link" rgba="0.75 0.75 0.75 1.0" />
-                                        </body>
-                                    </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>
-
-                                # narrow helper
-                                <body name="picker_narrow_loop_link" pos="0 -0.021633 0.07387" quat="0.0 -0.707 0.0 -0.707">
-                                    <inertial pos="0.014869 -0.0036066 0.00029307" quat="0.800794 2.10378e-06 -6.78039e-07 0.59894" mass="0.022591" diaginertia="4.76981e-06 4.35879e-06 6.0445e-07"/>
-                                    <joint name="hand_narrow_loop_joint" pos="0 0 0" axis="0 0 -1" limited="auto" armature="0.0" damping="0.01" frictionloss="0.01" />
-                                    <geom type="mesh" mesh="picker_narrow_loop_link" rgba="0.75 0.75 0.75 1.0" />
+                                <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>
 
-                                # wide gripper
-                                <body name="picker_wide1_link" pos="0 0.0195 0.0565" quat="0.0 -0.707 0.0 -0.707">
-                                    <inertial pos="0.0095051 -0.0068479 6.8268e-05" quat="0.872477 -0.000843631 -0.000472499 0.488655" mass="0.025428" diaginertia="2.65502e-06 1.69428e-06 1.5501e-06"/>
-                                    <joint name="hand_wide1_joint" pos="0 0 0" axis="0 0 -1" range="0.0 0.8" limited="true" armature="0.1" damping="1.0" frictionloss="0.1" />
-                                    <geom type="mesh" mesh="picker_wide1_link" rgba="0.75 0.75 0.75 1.0" />
 
-                                    <body name="picker_wide2_link" pos="0.030852 -0.018551 0">
-                                        <inertial pos="0.0088027 0.007035 -1.6424e-05" quat="-0.251356 0.657548 0.259339 0.661207" mass="0.0040132" diaginertia="1.70091e-07 1.66561e-07 5.03584e-08"/>
-                                        <joint name="hand_wide2_joint" pos="0 0 0" axis="0 0 -1" range="-1.57 1.57" limited="true" armature="0.001" damping="1.0" frictionloss="0.1" springref="0.01" stiffness="1.0" />
-                                        <geom type="mesh" mesh="picker_wide2_link" rgba="0.75 0.75 0.75 1.0" />
+                                # 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="picker_wide3_link" pos="0.018118 0.01574 0">
-                                            <inertial pos="0.016206 0.0094593 4.7668e-05" quat="0.574355 3.4175e-06 6.47256e-06 0.818606" mass="0.035835" diaginertia="1.16461e-05 8.09447e-06 4.6309e-06"/>
-                                            <joint name="hand_wide3_joint" pos="0 0 0" axis="0 0 -1" range="-0.5 0.5" limited="true" armature="0.001" damping="1.0" frictionloss="0.1" />
-                                            <geom type="mesh" mesh="picker_wide3_link" rgba="0.75 0.75 0.75 1.0" />
-                                        </body>
+                                    <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>
 
-                                # wide helper
-                                <body name="picker_wide_loop_link" pos="0 0.021633 0.07387" quat="0.0 -0.707 0.0 -0.707">
-                                    <inertial pos="0.016268 0.0040555 0.00030323" quat="0.599119 -2.137e-06 -5.96717e-07 0.80066" mass="0.025142" diaginertia="6.26903e-06 5.85397e-06 6.2389e-07"/>
-                                    <joint name="hand_wide_loop_joint" pos="0 0 0" axis="0 0 -1" limited="auto" armature="0.0" damping="0.01" frictionloss="0.01" />
-                                    <geom type="mesh" mesh="picker_wide_loop_link" rgba="0.75 0.75 0.75 1.0" />
+                                # 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>

hex_zmq_servers/mujoco/archer_d6y/model/scene.xml

@@ -9,7 +9,7 @@
         <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"/>
+        <map znear="0.001" zfar="50"/>
     </visual>
 
     <asset>
@@ -41,11 +41,11 @@
     <include file="setting.xml"/>
 
     <keyframe>
-        # qpos: 14 + 7
-        # ctrl: (6 + 2)
+        # 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.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 0.0"/>
     </keyframe>
 </mujoco>

hex_zmq_servers/mujoco/archer_d6y/model/setting.xml

@@ -5,33 +5,33 @@
         <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="picker_base_link" />
-        <exclude body1="picker_base_link" body2="picker_narrow1_link" />
-        <exclude body1="picker_narrow1_link" body2="picker_narrow2_link" />
-        <exclude body1="picker_narrow2_link" body2="picker_narrow3_link" />
-        <exclude body1="picker_narrow3_link" body2="picker_narrow_loop_link" />
-        <exclude body1="picker_narrow_loop_link" body2="picker_base_link" />
-        <exclude body1="picker_base_link" body2="picker_wide1_link" />
-        <exclude body1="picker_wide1_link" body2="picker_wide2_link" />
-        <exclude body1="picker_wide2_link" body2="picker_wide3_link" />
-        <exclude body1="picker_wide3_link" body2="picker_wide_loop_link" />
-        <exclude body1="picker_wide_loop_link" body2="picker_base_link" />
+        <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>
-        <connect body1="picker_narrow_loop_link" body2="picker_narrow3_link" anchor="0.03153 -0.00975 0.0" />
-        <connect body1="picker_wide_loop_link" body2="picker_wide3_link" anchor="0.03153 0.00975 0.0" />
+        <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>
-        <position joint="joint_1" kp="1500" kv="15.0" ctrllimited="true" ctrlrange="-2.7 2.7" forcerange="-30 30"/>
-        <position joint="joint_2" kp="1500" kv="15.0" ctrllimited="true" ctrlrange="-1.57 2.09" forcerange="-30 30"/>
-        <position joint="joint_3" kp="1500" kv="15.0" ctrllimited="true" ctrlrange="0 3.14" forcerange="-30 30"/>
-        <position joint="joint_4" kp="1500" kv="15.0" ctrllimited="true" ctrlrange="-1.57 1.57" forcerange="-30 30"/>
-        <position joint="joint_5" kp="1500" kv="15.0" ctrllimited="true" ctrlrange="-1.57 1.57" forcerange="-30 30"/>
-        <position joint="joint_6" kp="1500" kv="15.0" ctrllimited="true" ctrlrange="-1.57 1.57" forcerange="-30 30"/>
-        <position joint="hand_narrow1_joint" kp="1500" kv="15.0" ctrllimited="true" ctrlrange="0.0 0.8" forcerange="-20 20"/>
-        <position joint="hand_wide1_joint" kp="1500" kv="15.0" ctrllimited="true" ctrlrange="0.0 0.8" forcerange="-20 20"/>
+        <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_d6y/mujoco_archer_d6y.py

@@ -21,10 +21,14 @@ from ...zmq_base import (
     HexRate,
     HexSafeValue,
 )
+from hex_robo_utils import HexCtrlUtilMit as CtrlUtil
 
 MUJOCO_CONFIG = {
     "states_rate": 250,
     "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,
 }
@@ -41,6 +45,9 @@ class HexMujocoArcherD6y(HexMujocoBase):
         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.__headless = mujoco_config["headless"]
             self.__sens_ts = mujoco_config["sens_ts"]
         except KeyError as ke:
@@ -56,13 +63,19 @@ class HexMujocoArcherD6y(HexMujocoBase):
 
         # state init
         self.__state_robot_idx = [0, 1, 2, 3, 4, 5, 6]
-        self.__state_obj_idx = [14, 15, 16, 17, 18, 19, 20]
+        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,
@@ -80,10 +93,10 @@ class HexMujocoArcherD6y(HexMujocoBase):
         self.__states_trig_thresh = int(self.__sim_rate / states_rate)
 
         # camera init
-        width, height = 400, 400
+        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, width / 2, height / 2])
+        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()
@@ -188,9 +201,8 @@ class HexMujocoArcherD6y(HexMujocoBase):
         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]] = 0.8 - pos[self.__state_robot_idx[-1]]
-        vel[self.__state_robot_idx[-1]] *= -1
-        eff[self.__state_robot_idx[-1]] *= -1
+        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],
@@ -199,10 +211,32 @@ class HexMujocoArcherD6y(HexMujocoBase):
         ]).T, self.__data.qpos[self.__state_obj_idx].copy()
 
     def __set_cmds(self, cmds: np.ndarray):
-        self.__data.ctrl[self.__ctrl_robot_idx] = cmds
-        self.__data.ctrl[self.__ctrl_robot_idx[-1]] = 0.8 - cmds[-1]
-        self.__data.ctrl[self.__ctrl_robot_idx[-1] +
-                         1] = self.__data.ctrl[self.__ctrl_robot_idx[-1]]
+        tau_cmds = None
+        if not self.__tau_ctrl:
+            if len(cmds.shape) == 1:
+                cmd_pos = cmds.copy()
+                cmd_tor = np.zeros_like(cmds)
+            else:
+                cmd_pos = cmds[:, 0].copy()
+                cmd_tor = cmds[:, 1].copy()
+            cmd_pos = self._apply_pos_limits(
+                cmd_pos,
+                self._limits[0, :, 0],
+                self._limits[0, :, 1],
+            )
+            cmd_pos[-1] /= self.__gripper_ratio
+            tau_cmds = self.__mit_ctrl(
+                self.__mit_kp,
+                self.__mit_kd,
+                cmd_pos,
+                np.zeros(len(self.__state_robot_idx)),
+                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")

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

@@ -6,15 +6,13 @@
     <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="picker_base_link" file="picker_base_link.STL"/>
-    <mesh name="picker_narrow1_link" file="picker_narrow1_link.STL"/>
-    <mesh name="picker_narrow2_link" file="picker_narrow2_link.STL"/>
-    <mesh name="picker_narrow3_link" file="picker_narrow3_link.STL"/>
-    <mesh name="picker_narrow_loop_link" file="picker_narrow_loop_link.STL"/>
-    <mesh name="picker_wide1_link" file="picker_wide1_link.STL"/>
-    <mesh name="picker_wide2_link" file="picker_wide2_link.STL"/>
-    <mesh name="picker_wide3_link" file="picker_wide3_link.STL"/>
-    <mesh name="picker_wide_loop_link" file="picker_wide_loop_link.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>