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

hex_zmq_servers/mujoco/mujoco_base.py

@@ -25,6 +25,8 @@ NET_CONFIG = {
     "server_num_workers": 4,
 }
 
+TAU = 2 * np.pi
+
 
 class HexMujocoBase(HexDeviceBase):
 
@@ -52,6 +54,38 @@ class HexMujocoBase(HexDeviceBase):
         self._wait_for_working()
         return self._intri
 
+    @staticmethod
+    def _rads_normalize(rads: np.ndarray) -> np.ndarray:
+        return (rads + np.pi) % TAU - np.pi
+
+    @staticmethod
+    def _apply_pos_limits(
+        rads: np.ndarray,
+        lower_bound: np.ndarray,
+        upper_bound: np.ndarray,
+    ) -> np.ndarray:
+        normed_rads = HexMujocoBase._rads_normalize(rads)
+        outside = (normed_rads < lower_bound) | (normed_rads > upper_bound)
+        if not np.any(outside):
+            return normed_rads
+
+        lower_dist = np.fabs(
+            HexMujocoBase._rads_normalize(
+                (normed_rads - lower_bound)[outside]))
+        upper_dist = np.fabs(
+            HexMujocoBase._rads_normalize(
+                (normed_rads - upper_bound)[outside]))
+        choose_lower = lower_dist < upper_dist
+        choose_upper = ~choose_lower
+
+        outside_full = np.flatnonzero(outside)
+        outside_lower = outside_full[choose_lower]
+        outside_upper = outside_full[choose_upper]
+        normed_rads[outside_lower] = lower_bound[outside_lower]
+        normed_rads[outside_upper] = upper_bound[outside_upper]
+
+        return normed_rads
+
     @abstractmethod
     def work_loop(self, hex_values: list[HexSafeValue]):
         raise NotImplementedError(

hex_zmq_servers/robot/__init__.py

@@ -9,9 +9,12 @@
 from .robot_base import HexRobotBase, HexRobotClientBase, HexRobotServerBase
 from .dummy import HexRobotDummy, HexRobotDummyClient, HexRobotDummyServer
 from .gello import HexRobotGello, HexRobotGelloClient, HexRobotGelloServer
-from .hexarm import HexRobotHexarm, HexRobotHexarmClient, HexRobotHexarmServer
+from .hexarm import HexRobotHexarm, HexRobotHexarmClient, HexRobotHexarmServer, HEXARM_URDF_PATH_DICT
 
 __all__ = [
+    # path
+    "HEXARM_URDF_PATH_DICT",
+
     # base
     "HexRobotBase",
     "HexRobotClientBase",

hex_zmq_servers/robot/hexarm/__init__.py

@@ -10,7 +10,37 @@ from .robot_hexarm import HexRobotHexarm
 from .robot_hexarm_cli import HexRobotHexarmClient
 from .robot_hexarm_srv import HexRobotHexarmServer
 
+import os
+
+urdf_dir = os.path.join(os.path.dirname(__file__), "urdf")
+HEXARM_URDF_PATH_DICT = {
+    "archer_d6y_empty":
+    f"{urdf_dir}/archer_d6y/empty.urdf",
+    "archer_d6y_gp100":
+    f"{urdf_dir}/archer_d6y/gp100.urdf",
+    "archer_d6y_gp100_handle":
+    f"{urdf_dir}/archer_d6y/gp100_handle.urdf",
+    "archer_d6y_gp100_p050":
+    f"{urdf_dir}/archer_d6y/gp100_p050.urdf",
+    "archer_d6y_gp100_p050_handle":
+    f"{urdf_dir}/archer_d6y/gp100_p050_handle.urdf",
+    "archer_l6y_empty":
+    f"{urdf_dir}/archer_l6y/empty.urdf",
+    "archer_l6y_gp100":
+    f"{urdf_dir}/archer_l6y/gp100.urdf",
+    "archer_l6y_gp100_handle":
+    f"{urdf_dir}/archer_l6y/gp100_handle.urdf",
+    "archer_l6y_gp100_p050":
+    f"{urdf_dir}/archer_l6y/gp100_p050.urdf",
+    "archer_l6y_gp100_p050_handle":
+    f"{urdf_dir}/archer_l6y/gp100_p050_handle.urdf",
+}
+
 __all__ = [
+    # path
+    "HEXARM_URDF_PATH_DICT",
+
+    # robot
     "HexRobotHexarm",
     "HexRobotHexarmClient",
     "HexRobotHexarmServer",

hex_zmq_servers/robot/hexarm/robot_hexarm.py

@@ -32,7 +32,7 @@ ROBOT_CONFIG = {
 
 HEX_DEVICE_TYPE_DICT = {
     "archer_d6y": 16,
-    "os_d6y": 16,
+    "archer_l6y": 16,
 }
 
 
@@ -58,11 +58,11 @@ class HexRobotHexarm(HexRobotBase):
 
         self.__mit_kp = robot_config.get(
             "mit_kp",
-            [200.0, 200.0, 200.0, 75.0, 15.0, 15.0],
+            [200.0, 200.0, 200.0, 75.0, 15.0, 15.0, 20.0],
         )
         self.__mit_kd = robot_config.get(
             "mit_kd",
-            [12.5, 12.5, 12.5, 6.0, 0.31, 0.31],
+            [12.5, 12.5, 12.5, 6.0, 0.31, 0.31, 1.0],
         )
 
         # variables
@@ -82,6 +82,7 @@ class HexRobotHexarm(HexRobotBase):
             print("\033[33mArm not found\033[0m")
             time.sleep(1)
         self.__arm_archer = self.__hex_api.find_device_by_robot_type(arm_type)
+        self.__arm_archer.start()
         self.__arm_archer.command_timeout_check(False)
         self.__arm_dofs = len(self.__arm_archer)
         self._limits = self.__arm_archer.get_joint_limits()
@@ -224,4 +225,5 @@ class HexRobotHexarm(HexRobotBase):
         return True
 
     def close(self):
+        self.__arm_archer.stop()
         self.__hex_api.close()

hex_zmq_servers/robot/hexarm/robot_hexarm_cli.py

@@ -20,7 +20,7 @@ ROBOT_CONFIG = {
     "device_ip": "172.18.8.161",
     "device_port": 8439,
     "control_hz": 250,
-    "arm_type": 16,
+    "arm_type": "archer_l6y",
     "sens_ts": True,
 }
 

hex_zmq_servers/robot/hexarm/robot_hexarm_srv.py

@@ -33,7 +33,7 @@ ROBOT_CONFIG = {
     "device_ip": "172.18.8.161",
     "device_port": 8439,
     "control_hz": 250,
-    "arm_type": 16,
+    "arm_type": "archer_l6y",
     "use_gripper": True,
     "mit_kp": [200.0, 200.0, 200.0, 75.0, 15.0, 15.0, 0.0],
     "mit_kd": [12.5, 12.5, 12.5, 6.0, 0.31, 0.31, 0.0],

hex_zmq_servers/robot/hexarm/urdf/archer_d6y/empty.urdf

@@ -0,0 +1,207 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="archer_d6y">
+
+    <link name="base_link">
+        <inertial>
+            <origin xyz="-0.001 0 0.322" rpy="0 0 0" />
+            <mass value="0.687" />
+            <inertia ixx="0.0005" iyy="0.0005" izz="0.0007" ixy="0" ixz="0" iyz="0" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/base_link.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/base_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+
+    <link name="link_1">
+        <inertial>
+            <origin xyz="0.008 -0.0023 0.0213" rpy="0 0 0" />
+            <mass value="0.2154" />
+            <inertia ixx="0.0003" iyy="0.0002" izz="0.0003" ixy="0" ixz="0" iyz="0"/>
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_1.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_1.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_1" type="revolute">
+        <origin xyz="0 0 0.0665" rpy="0 0 0" />
+        <parent link="base_link" />
+        <child link="link_1" />
+        <axis xyz="0 0 1" />
+        <limit lower="-2.61" upper="3.14" effort="6" velocity="6" />
+    </joint>
+
+    <link name="link_2">
+        <inertial>
+            <origin xyz="0.0001 -0.0028 0.1317" rpy="0 0 0" />
+            <mass value="1.3123" />
+            <inertia ixx="0.018" iyy="0.0177" izz="0.0007" ixy="0" ixz="0" iyz="0.0007" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_2.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_2.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_2" type="revolute">
+        <origin xyz="0.0199999999999999 0 0.0450000000000004" rpy="0 0 0" />
+        <parent link="link_1" />
+        <child link="link_2" />
+        <axis xyz="0 1 0" />
+        <limit lower="-1.57" upper="2.09" effort="6" velocity="6" />
+    </joint>
+
+    <link name="link_3">
+        <inertial>
+            <origin xyz="-0.051 0.0008 0.1447" rpy="0 0 0" />
+            <mass value="1.1083" />
+            <inertia ixx="0.0114" iyy="0.0112" izz="0.0016" ixy="-0.0001" ixz="0.0014" iyz="0.0006" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_3.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_3.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_3" type="revolute">
+        <origin xyz="0 0 0.264" rpy="0 0 0" />
+        <parent link="link_2" />
+        <child link="link_3" />
+        <axis xyz="0 1 0" />
+        <limit lower="0" upper="3.14" effort="6" velocity="6" />
+    </joint>
+
+    <link name="link_4">
+        <inertial>
+            <origin xyz="-0.0597 -0.001 0.0572" rpy="0 0 0" />
+            <mass value="0.5398" />
+            <inertia ixx="0.0008" iyy="0.001" izz="0.0008" ixy="0" ixz="0.0003" iyz="0" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_4.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_4.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_4" type="revolute">
+        <origin xyz="-0.0599999999999998 0 0.245" rpy="0 0 0" />
+        <parent link="link_3" />
+        <child link="link_4" />
+        <axis xyz="0 1 0" />
+        <limit lower="-1.57" upper="1.57" effort="6" velocity="6" />
+    </joint>
+
+    <link name="link_5">
+        <inertial>
+            <origin xyz="0.0292 0.0001 -0.0107" rpy="0 0 0" />
+            <mass value="0.3984" />
+            <inertia ixx="0.0002" iyy="0.0003" izz="0.0002" ixy="0" ixz="0" iyz="0" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_5.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_5.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_5" type="revolute">
+        <origin xyz="-0.0553005693392305 0 0.0702500000000025" rpy="0 0 0" />
+        <parent link="link_4" />
+        <child link="link_5" />
+        <axis xyz="-1 0 0" />
+        <limit lower="-1.57" upper="1.57" effort="6" velocity="6" />
+    </joint>
+
+    <link name="link_6">
+        <inertial>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <mass value="0.001" />
+            <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_6.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_6.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_6" type="revolute">
+        <origin xyz="0.0578000000000073 0 0.0289500000000001" rpy="0 0 0" />
+        <parent link="link_5" />
+        <child link="link_6" />
+        <axis xyz="0 0 1" />
+        <limit lower="-2.96" upper="2.96" effort="6" velocity="6" />
+    </joint>
+
+</robot>

hex_zmq_servers/robot/hexarm/urdf/archer_d6y/gp100.urdf

@@ -0,0 +1,207 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="archer_d6y">
+
+    <link name="base_link">
+        <inertial>
+            <origin xyz="-0.001 0 0.322" rpy="0 0 0" />
+            <mass value="0.687" />
+            <inertia ixx="0.0005" iyy="0.0005" izz="0.0007" ixy="0" ixz="0" iyz="0" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/base_link.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/base_link.STL" />
+            </geometry>
+        </collision>
+    </link>
+
+    <link name="link_1">
+        <inertial>
+            <origin xyz="0.008 -0.0023 0.0213" rpy="0 0 0" />
+            <mass value="0.2154" />
+            <inertia ixx="0.0003" iyy="0.0002" izz="0.0003" ixy="0" ixz="0" iyz="0"/>
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_1.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_1.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_1" type="revolute">
+        <origin xyz="0 0 0.0665" rpy="0 0 0" />
+        <parent link="base_link" />
+        <child link="link_1" />
+        <axis xyz="0 0 1" />
+        <limit lower="-2.61" upper="3.14" effort="6" velocity="6" />
+    </joint>
+
+    <link name="link_2">
+        <inertial>
+            <origin xyz="0.0001 -0.0028 0.1317" rpy="0 0 0" />
+            <mass value="1.3123" />
+            <inertia ixx="0.018" iyy="0.0177" izz="0.0007" ixy="0" ixz="0" iyz="0.0007" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_2.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_2.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_2" type="revolute">
+        <origin xyz="0.0199999999999999 0 0.0450000000000004" rpy="0 0 0" />
+        <parent link="link_1" />
+        <child link="link_2" />
+        <axis xyz="0 1 0" />
+        <limit lower="-1.57" upper="2.09" effort="6" velocity="6" />
+    </joint>
+
+    <link name="link_3">
+        <inertial>
+            <origin xyz="-0.051 0.0008 0.1447" rpy="0 0 0" />
+            <mass value="1.1083" />
+            <inertia ixx="0.0114" iyy="0.0112" izz="0.0016" ixy="-0.0001" ixz="0.0014" iyz="0.0006" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_3.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_3.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_3" type="revolute">
+        <origin xyz="0 0 0.264" rpy="0 0 0" />
+        <parent link="link_2" />
+        <child link="link_3" />
+        <axis xyz="0 1 0" />
+        <limit lower="0" upper="3.14" effort="6" velocity="6" />
+    </joint>
+
+    <link name="link_4">
+        <inertial>
+            <origin xyz="-0.0597 -0.001 0.0572" rpy="0 0 0" />
+            <mass value="0.5398" />
+            <inertia ixx="0.0008" iyy="0.001" izz="0.0008" ixy="0" ixz="0.0003" iyz="0" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_4.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_4.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_4" type="revolute">
+        <origin xyz="-0.0599999999999998 0 0.245" rpy="0 0 0" />
+        <parent link="link_3" />
+        <child link="link_4" />
+        <axis xyz="0 1 0" />
+        <limit lower="-1.57" upper="1.57" effort="6" velocity="6" />
+    </joint>
+
+    <link name="link_5">
+        <inertial>
+            <origin xyz="0.0292 0.0001 -0.0107" rpy="0 0 0" />
+            <mass value="0.3984" />
+            <inertia ixx="0.0002" iyy="0.0003" izz="0.0002" ixy="0" ixz="0" iyz="0" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_5.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_5.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_5" type="revolute">
+        <origin xyz="-0.0553005693392305 0 0.0702500000000025" rpy="0 0 0" />
+        <parent link="link_4" />
+        <child link="link_5" />
+        <axis xyz="-1 0 0" />
+        <limit lower="-1.57" upper="1.57" effort="6" velocity="6" />
+    </joint>
+
+    <link name="link_6">
+        <inertial>
+            <origin xyz="-0.0009 0.0001 0.0596" rpy="0 0 0" />
+            <mass value="0.6760" />
+            <inertia ixx="0.0011" ixy="0" ixz="0" iyy="0.0011" iyz="0" izz="0.0004" />
+        </inertial>
+        <visual>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_6.STL" />
+            </geometry>
+            <material name="">
+                <color rgba="1 1 1 1" />
+            </material>
+        </visual>
+        <collision>
+            <origin xyz="0 0 0" rpy="0 0 0" />
+            <geometry>
+                <mesh filename="package://xpkg_urdf_archer_d6y/meshes/link_6.STL" />
+            </geometry>
+        </collision>
+    </link>
+    <joint name="joint_6" type="revolute">
+        <origin xyz="0.0578000000000073 0 0.0289500000000001" rpy="0 0 0" />
+        <parent link="link_5" />
+        <child link="link_6" />
+        <axis xyz="0 0 1" />
+        <limit lower="-2.96" upper="2.96" effort="6" velocity="6" />
+    </joint>
+
+</robot>