hex-zmq-servers 0.3.16__py3-none-any.whl → 1.0.1__py3-none-any.whl

hex_zmq_servers/__init__.py

@@ -15,7 +15,6 @@ from .zmq_base import HexZMQDummyClient, HexZMQDummyServer
 
 from .robot import HexRobotBase, HexRobotClientBase, HexRobotServerBase
 from .robot import HexRobotDummy, HexRobotDummyClient, HexRobotDummyServer
-from .robot import HexRobotGello, HexRobotGelloClient, HexRobotGelloServer
 from .robot import HexRobotHexarm, HexRobotHexarmClient, HexRobotHexarmServer, HEXARM_URDF_PATH_DICT
 
 from .cam import HexCamBase, HexCamClientBase, HexCamServerBase
@@ -28,7 +27,6 @@ file_dir = os.path.dirname(os.path.abspath(__file__))
 HEX_ZMQ_SERVERS_PATH_DICT = {
     "zmq_dummy": f"{file_dir}/zmq_base.py",
     "robot_dummy": f"{file_dir}/robot/dummy/robot_dummy_srv.py",
-    "robot_gello": f"{file_dir}/robot/gello/robot_gello_srv.py",
     "robot_hexarm": f"{file_dir}/robot/hexarm/robot_hexarm_srv.py",
     "cam_dummy": f"{file_dir}/cam/dummy/cam_dummy_srv.py",
     "cam_rgb": f"{file_dir}/cam/rgb/cam_rgb_srv.py",
@@ -36,7 +34,6 @@ HEX_ZMQ_SERVERS_PATH_DICT = {
 HEX_ZMQ_CONFIGS_PATH_DICT = {
     "zmq_dummy": f"{file_dir}/config/zmq_dummy.json",
     "robot_dummy": f"{file_dir}/config/robot_dummy.json",
-    "robot_gello": f"{file_dir}/config/robot_gello.json",
     "robot_hexarm": f"{file_dir}/config/robot_hexarm.json",
     "cam_dummy": f"{file_dir}/config/cam_dummy.json",
     "cam_rgb": f"{file_dir}/config/cam_rgb.json",
@@ -80,9 +77,6 @@ __all__ = [
     "HexRobotDummy",
     "HexRobotDummyClient",
     "HexRobotDummyServer",
-    "HexRobotGello",
-    "HexRobotGelloClient",
-    "HexRobotGelloServer",
     "HexRobotHexarm",
     "HexRobotHexarmClient",
     "HexRobotHexarmServer",
@@ -104,6 +98,7 @@ from importlib.util import find_spec
 
 _HAS_BERXEL = find_spec("berxel_py_wrapper") is not None
 _HAS_REALSENSE = find_spec("pyrealsense2") is not None
+_HAS_DYNAMIXEL = find_spec("dynamixel-sdk") is not None
 _HAS_MUJOCO = find_spec("mujoco") is not None
 
 # Optional: berxel
@@ -140,6 +135,23 @@ if _HAS_REALSENSE:
         "HexCamRealsenseServer",
     ])
 
+# Optional: dynamixel
+if _HAS_DYNAMIXEL:
+    from .robot import HexRobotGello, HexRobotGelloClient, HexRobotGelloServer
+    HEX_ZMQ_SERVERS_PATH_DICT.update({
+        "robot_gello":
+        f"{file_dir}/robot/gello/robot_gello_srv.py",
+    })
+    HEX_ZMQ_CONFIGS_PATH_DICT.update({
+        "robot_gello":
+        f"{file_dir}/config/robot_gello.json",
+    })
+    __all__.extend([
+        "HexRobotGello",
+        "HexRobotGelloClient",
+        "HexRobotGelloServer",
+    ])
+
 # Optional: mujoco
 if _HAS_MUJOCO:
     from .mujoco import HexMujocoBase, HexMujocoClientBase, HexMujocoServerBase

hex_zmq_servers/cam/berxel/cam_berxel.py

@@ -47,8 +47,8 @@ class HexCamBerxel(HexCamBase):
 
         # variables
         # berxel variables
-        self.__context = None
-        self.__device = None
+        self.__context: BerxelHawkContext | None = None
+        self.__device: BerxelHawkDevice | None = None
         # camera variables
         self.__intri = np.zeros(4)
 
@@ -214,7 +214,15 @@ class HexCamBerxel(HexCamBase):
         return True
 
     def __start_stream(self):
-        self.__device.setColorExposureGain(self.__exposure, self.__gain)
+        if self.__serial_number.startswith('P008'):
+            self.__device.setSonixAEStatus(False)
+            self.__device.setSonixExposureTime(int(self.__exposure // 100))
+        else:
+            self.__device.setColorExposureGain(self.__exposure, self.__gain)
+        self.__device.setDepthElectricCurrent(700)
+        self.__device.setDepthAE(False)
+        self.__device.setDepthExposure(43)
+        self.__device.setDepthGain(1)
         self.__device.setRegistrationEnable(True)
         self.__device.setFrameSync(True)
         while self.__device.setSystemClock() != 0:

hex_zmq_servers/config/robot_hexarm.json

@@ -13,7 +13,6 @@
         "device_port": 8439,
         "control_hz": 1000,
         "arm_type": "archer_l6y",
-        "use_gripper": true,
         "mit_kp": [
             200.0,
             200.0,
@@ -21,7 +20,13 @@
             75.0,
             15.0,
             15.0,
-            20.0
+            20.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
         ],
         "mit_kd": [
             12.5,
@@ -30,7 +35,13 @@
             6.0,
             0.31,
             0.31,
-            1.0
+            1.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0
         ],
         "sens_ts": true
     }

hex_zmq_servers/mujoco/archer_y6/mujoco_archer_y6.py

@@ -76,21 +76,36 @@ class HexMujocoArcherY6(HexMujocoBase):
         mujoco.mj_resetData(self.__model, self.__data)
 
         # 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,
-        )
+        self.__state_idx = {
+            "robot_arm": ([0, 1, 2, 3, 4, 5]),
+            "robot_gripper": [6],
+            "obj": [12, 13, 14, 15, 16, 17, 18],
+        }
+        self.__ctrl_idx = {
+            "robot_arm": [0, 1, 2, 3, 4, 5],
+            "robot_gripper": [6],
+        }
+        self.__limit_idx = {
+            "robot_arm":
+            np.arange(len(self.__state_idx["robot_arm"])).tolist(),
+            "robot_gripper":
+            (np.arange(len(self.__state_idx["robot_gripper"])) +
+             len(self.__state_idx["robot_arm"])).tolist(),
+        }
+        print(f"self.__limit_idx: {self.__limit_idx}")
+        self._limits = self.__model.jnt_range[np.concatenate(
+            [self.__state_idx["robot_arm"], self.
+             __state_idx["robot_gripper"]]), :].copy().reshape(-1, 1, 2)
         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)])
+        self._dofs = np.array([
+            len(self.__state_idx["robot_arm"]),
+            len(self.__state_idx["robot_gripper"])
+        ])
         keyframe_id = mujoco.mj_name2id(
             self.__model,
             mujoco.mjtObj.mjOBJ_KEY,
@@ -242,16 +257,25 @@ class HexMujocoArcherY6(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]] = pos[
-            self.__state_robot_idx[-1]] * self.__gripper_ratio
+        pos[self.__state_idx["robot_gripper"]] = pos[
+            self.__state_idx["robot_gripper"]] * 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()
+            pos[self.__state_idx["robot_arm"] +
+                self.__state_idx["robot_gripper"]],
+            vel[self.__state_idx["robot_arm"] +
+                self.__state_idx["robot_gripper"]],
+            eff[self.__state_idx["robot_arm"] +
+                self.__state_idx["robot_gripper"]],
+        ]).T, self.__data.qpos[self.__state_idx["obj"]].copy()
 
     def __set_cmds(self, cmds: np.ndarray):
+        state_idx = self.__state_idx["robot_arm"] + self.__state_idx[
+            "robot_gripper"]
+        ctrl_idx = self.__ctrl_idx["robot_arm"] + self.__ctrl_idx[
+            "robot_gripper"]
+        limit_idx = self.__limit_idx["robot_arm"] + self.__limit_idx[
+            "robot_gripper"]
         tau_cmds = None
         if not self.__tau_ctrl:
             cmd_pos = None
@@ -278,8 +302,8 @@ class HexMujocoArcherY6(HexMujocoBase):
                 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],
+                self._limits[limit_idx, 0, 0],
+                self._limits[limit_idx, 0, 1],
             )
             tar_pos[-1] /= self.__gripper_ratio
             tau_cmds = self.__mit_ctrl(
@@ -287,13 +311,13 @@ class HexMujocoArcherY6(HexMujocoBase):
                 cmd_kd,
                 tar_pos,
                 tar_vel,
-                self.__data.qpos[self.__state_robot_idx],
-                self.__data.qvel[self.__state_robot_idx],
+                self.__data.qpos[state_idx],
+                self.__data.qvel[state_idx],
                 cmd_tor,
             )
         else:
             tau_cmds = cmds.copy()
-        self.__data.ctrl[self.__ctrl_robot_idx] = tau_cmds
+        self.__data.ctrl[ctrl_idx] = tau_cmds
 
     def __get_rgb(self):
         self.__rgb_cam.update_scene(self.__data, "end_camera")

hex_zmq_servers/mujoco/e3_desktop/mujoco_e3_desktop.py

@@ -77,18 +77,37 @@ class HexMujocoE3Desktop(HexMujocoBase):
         mujoco.mj_resetData(self.__model, self.__data)
 
         # state init
-        self.__state_left_idx = [0, 1, 2, 3, 4, 5, 6]
-        self.__state_right_idx = [12, 13, 14, 15, 16, 17, 18]
-        self.__obj_pose_idx = [24, 25, 26, 27, 28, 29, 30]
-        self.__ctrl_left_idx = [0, 1, 2, 3, 4, 5, 6]
-        self.__ctrl_right_idx = [7, 8, 9, 10, 11, 12, 13]
-        self._limits = np.stack(
-            [
-                self.__model.jnt_range[self.__state_left_idx, :],
-                self.__model.jnt_range[self.__state_right_idx, :]
-            ],
-            axis=0,
-        )
+        self.__state_idx = {
+            "left_arm": [0, 1, 2, 3, 4, 5],
+            "left_gripper": [6],
+            "right_arm": [12, 13, 14, 15, 16, 17],
+            "right_gripper": [18],
+            "obj": [24, 25, 26, 27, 28, 29, 30],
+        }
+        self.__ctrl_idx = {
+            "left_arm": [0, 1, 2, 3, 4, 5],
+            "left_gripper": [6],
+            "right_arm": [7, 8, 9, 10, 11, 12],
+            "right_gripper": [13],
+        }
+        self.__limit_idx = {
+            "left_arm":
+            np.arange(len(self.__state_idx["left_arm"])).tolist(),
+            "left_gripper": (np.arange(len(self.__state_idx["left_gripper"])) +
+                             len(self.__state_idx["left_arm"])).tolist(),
+            "right_arm": (np.arange(len(self.__state_idx["right_arm"])) +
+                          len(self.__state_idx["left_arm"]) +
+                          len(self.__state_idx["left_gripper"])).tolist(),
+            "right_gripper":
+            (np.arange(len(self.__state_idx["right_gripper"])) +
+             len(self.__state_idx["left_arm"]) +
+             len(self.__state_idx["left_gripper"]) +
+             len(self.__state_idx["right_arm"])).tolist(),
+        }
+        self._limits = self.__model.jnt_range[np.concatenate([
+            self.__state_idx["left_arm"], self.__state_idx["left_gripper"],
+            self.__state_idx["right_arm"], self.__state_idx["right_gripper"]
+        ]), :].copy().reshape(-1, 1, 2)
         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))
@@ -97,8 +116,10 @@ class HexMujocoE3Desktop(HexMujocoBase):
         self._limits[0, -1] *= self.__gripper_ratio
         self._limits[1, -1] *= self.__gripper_ratio
         self._dofs = np.array([
-            len(self.__state_left_idx),
-            len(self.__state_right_idx),
+            len(self.__state_idx["left_arm"]),
+            len(self.__state_idx["left_gripper"]),
+            len(self.__state_idx["right_arm"]),
+            len(self.__state_idx["right_gripper"]),
         ])
         keyframe_id = mujoco.mj_name2id(
             self.__model,
@@ -347,35 +368,47 @@ class HexMujocoE3Desktop(HexMujocoBase):
         pos = copy.deepcopy(self.__data.qpos)
         vel = copy.deepcopy(self.__data.qvel)
         eff = copy.deepcopy(self.__data.qfrc_actuator)
-        pos[self.__state_left_idx[-1]] = pos[
-            self.__state_left_idx[-1]] * self.__gripper_ratio
-        pos[self.__state_right_idx[-1]] = pos[
-            self.__state_right_idx[-1]] * self.__gripper_ratio
+        pos[self.__state_idx["left_gripper"]] = pos[
+            self.__state_idx["left_gripper"]] * self.__gripper_ratio
+        pos[self.__state_idx["right_gripper"]] = pos[
+            self.__state_idx["right_gripper"]] * self.__gripper_ratio
         return self.__mujoco_ts() if self.__sens_ts else hex_zmq_ts_now(
         ), np.array([
-            pos[self.__state_left_idx],
-            vel[self.__state_left_idx],
-            eff[self.__state_left_idx],
+            pos[self.__state_idx["left_arm"] +
+                self.__state_idx["left_gripper"]],
+            vel[self.__state_idx["left_arm"] +
+                self.__state_idx["left_gripper"]],
+            eff[self.__state_idx["left_arm"] +
+                self.__state_idx["left_gripper"]],
         ]).T, np.array([
-            pos[self.__state_right_idx],
-            vel[self.__state_right_idx],
-            eff[self.__state_right_idx],
-        ]).T, self.__data.qpos[self.__obj_pose_idx].copy()
+            pos[self.__state_idx["right_arm"] +
+                self.__state_idx["right_gripper"]],
+            vel[self.__state_idx["right_arm"] +
+                self.__state_idx["right_gripper"]],
+            eff[self.__state_idx["right_arm"] +
+                self.__state_idx["right_gripper"]],
+        ]).T, self.__data.qpos[self.__state_idx["obj"]].copy()
 
     def __set_cmds(self, cmds: np.ndarray, robot_name: str):
-        ctrl_idx = []
-        state_idx = []
+        state_idx = None
+        ctrl_idx = None
         limit_idx = None
         if robot_name == "left":
-            ctrl_idx = self.__ctrl_left_idx
+            ctrl_idx = self.__ctrl_idx["left_arm"] + self.__ctrl_idx[
+                "left_gripper"]
             if not self.__tau_ctrl:
-                state_idx = self.__state_left_idx
-                limit_idx = 0
+                state_idx = self.__state_idx["left_arm"] + self.__state_idx[
+                    "left_gripper"]
+                limit_idx = self.__limit_idx["left_arm"] + self.__limit_idx[
+                    "left_gripper"]
         elif robot_name == "right":
-            ctrl_idx = self.__ctrl_right_idx
+            ctrl_idx = self.__ctrl_idx["right_arm"] + self.__ctrl_idx[
+                "right_gripper"]
             if not self.__tau_ctrl:
-                state_idx = self.__state_right_idx
-                limit_idx = 1
+                state_idx = self.__state_idx["right_arm"] + self.__state_idx[
+                    "right_gripper"]
+                limit_idx = self.__limit_idx["right_arm"] + self.__limit_idx[
+                    "right_gripper"]
         else:
             raise ValueError(f"unknown robot name: {robot_name}")
         tau_cmds = None
@@ -404,8 +437,8 @@ class HexMujocoE3Desktop(HexMujocoBase):
                 raise ValueError(f"The shape of cmds is invalid: {cmds.shape}")
             tar_pos = self._apply_pos_limits(
                 cmd_pos,
-                self._limits[limit_idx, :, 0],
-                self._limits[limit_idx, :, 1],
+                self._limits[limit_idx, 0, 0],
+                self._limits[limit_idx, 0, 1],
             )
             tar_pos[-1] /= self.__gripper_ratio
             tau_cmds = self.__mit_ctrl(

hex_zmq_servers/robot/__init__.py

@@ -8,7 +8,6 @@
 
 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, HEXARM_URDF_PATH_DICT
 
 __all__ = [
@@ -25,13 +24,22 @@ __all__ = [
     "HexRobotDummyClient",
     "HexRobotDummyServer",
 
-    # gello
-    "HexRobotGello",
-    "HexRobotGelloClient",
-    "HexRobotGelloServer",
-
     # hexarm
     "HexRobotHexarm",
     "HexRobotHexarmClient",
     "HexRobotHexarmServer",
 ]
+
+# Check optional dependencies availability
+from importlib.util import find_spec
+
+_HAS_DYNAMIXEL = find_spec("dynamixel-sdk") is not None
+
+# Optional: dynamixel
+if _HAS_DYNAMIXEL:
+    from .gello import HexRobotGello, HexRobotGelloClient, HexRobotGelloServer
+    __all__.extend([
+        "HexRobotGello",
+        "HexRobotGelloClient",
+        "HexRobotGelloServer",
+    ])

hex_zmq_servers/robot/hexarm/__init__.py

@@ -46,6 +46,8 @@ HEXARM_URDF_PATH_DICT = {
     f"{urdf_dir}/archer_l6y/gp100_p050_handle.urdf",
     "firefly_y6_empty":
     f"{urdf_dir}/firefly_y6/empty.urdf",
+    "archer_d6y_gp80":
+    f"{urdf_dir}/archer_d6y/gp80.urdf",
 }
 
 __all__ = [

hex_zmq_servers/robot/hexarm/robot_hexarm.py

@@ -18,7 +18,7 @@ from ...zmq_base import (
     HexRate,
 )
 from ...hex_launch import hex_log, HEX_LOG_LEVEL
-from hex_device import HexDeviceApi, MotorBase
+from hex_device import HexDeviceApi, Arm, Hands
 from hex_device.motor_base import CommandType
 
 ROBOT_CONFIG = {
@@ -26,7 +26,6 @@ ROBOT_CONFIG = {
     "device_port": 8439,
     "control_hz": 250,
     "arm_type": "archer_y6",
-    "use_gripper": True,
     "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],
     "sens_ts": True,
@@ -37,6 +36,7 @@ HEX_DEVICE_TYPE_DICT = {
     "archer_d6y": 16,
     "archer_l6y": 17,
     "firefly_y6": 27,
+    "hello": 26,
 }
 
 
@@ -54,7 +54,6 @@ class HexRobotHexarm(HexRobotBase):
             device_port = robot_config["device_port"]
             control_hz = robot_config["control_hz"]
             arm_type = HEX_DEVICE_TYPE_DICT[robot_config["arm_type"]]
-            use_gripper = robot_config["use_gripper"]
             self.__sens_ts = robot_config["sens_ts"]
         except KeyError as ke:
             missing_key = ke.args[0]
@@ -73,8 +72,8 @@ class HexRobotHexarm(HexRobotBase):
         # variables
         # hex_arm variables
         self.__hex_api: HexDeviceApi | None = None
-        self.__arm_archer: MotorBase | None = None
-        self.__gripper: MotorBase | None = None
+        self.__arm: Arm | None = None
+        self.__gripper: Hands | None = None
 
         # buffer
         self.__arm_state_buffer: dict | None = None
@@ -90,40 +89,46 @@ class HexRobotHexarm(HexRobotBase):
         while self.__hex_api.find_device_by_robot_type(arm_type) is None:
             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_dofs = len(self.__arm_archer)
-        self._limits = self.__arm_archer.get_joint_limits()
+        self.__arm = self.__hex_api.find_device_by_robot_type(arm_type)
+        self.__arm.start()
 
         # try to open gripper
-        self.__gripper_dofs = 0
-        self.__gripper = None
-        if use_gripper:
-            self.__gripper = self.__hex_api.find_optional_device_by_id(1)
-            if self.__gripper is not None:
-                self.__gripper_dofs = len(self.__gripper)
-                self._limits += [self.__gripper.get_joint_limits()]
-            else:
-                print("\033[33mGripper not found\033[0m")
+        self.__gripper = self.__hex_api.find_optional_device_by_id(1)
+        if self.__gripper is None:
+            print("\033[33mGripper not found\033[0m")
+
+        # variables init
+        arm_dofs = len(self.__arm)
+        self._dofs = [arm_dofs]
+        self._limits = np.array(self.__arm.get_joint_limits()).reshape(-1, 3, 2)
+        self.__motor_idx = {"robot_arm": np.arange(arm_dofs).tolist()}
+        if self.__gripper is not None:
+            gripper_dofs = len(self.__gripper)
+            self._dofs.append(gripper_dofs)
+            gripper_limits = np.array(self.__gripper.get_joint_limits()).reshape(-1, 3, 2)
+            self._limits = np.concatenate([self._limits, gripper_limits], axis=0)
+            self.__motor_idx["robot_gripper"] = (np.arange(gripper_dofs) +
+                                                 arm_dofs).tolist()
 
         # modify variables
-        self._dofs = [self.__arm_dofs + self.__gripper_dofs]
+        self._dofs = np.array(self._dofs)
+        self._dofs_sum = self._dofs.sum()
         self._limits = np.ascontiguousarray(np.asarray(self._limits)).reshape(
-            self._dofs[0], 3, 2)
+            self._dofs_sum, 3, 2)
         self.__mit_kp = np.ascontiguousarray(np.asarray(self.__mit_kp))
         self.__mit_kd = np.ascontiguousarray(np.asarray(self.__mit_kd))
-        if self.__mit_kp.shape[0] < self._dofs[0] or self.__mit_kd.shape[
-                0] < self._dofs[0]:
+        if self.__mit_kp.shape[0] < self._dofs_sum or self.__mit_kd.shape[
+                0] < self._dofs_sum:
             raise ValueError(
                 "The length of mit_kp and mit_kd must be greater than or equal to the number of motors"
             )
-        elif self.__mit_kp.shape[0] > self._dofs[0] or self.__mit_kd.shape[
-                0] > self._dofs[0]:
+        elif self.__mit_kp.shape[0] > self._dofs_sum or self.__mit_kd.shape[
+                0] > self._dofs_sum:
             print(
                 f"\033[33mThe length of mit_kp and mit_kd is greater than the number of motors\033[0m"
             )
-            self.__mit_kp = self.__mit_kp[:self._dofs[0]]
-            self.__mit_kd = self.__mit_kd[:self._dofs[0]]
+            self.__mit_kp = self.__mit_kp[:self._dofs_sum]
+            self.__mit_kd = self.__mit_kd[:self._dofs_sum]
 
         # start work loop
         self._working.set()
@@ -167,13 +172,12 @@ class HexRobotHexarm(HexRobotBase):
         self.close()
 
     def __get_states(self) -> tuple[np.ndarray | None, dict | None]:
-        if self.__arm_archer is None:
+        if self.__arm is None:
             return None, None
 
         # (arm_dofs, 3) # pos vel eff
         if self.__arm_state_buffer is None:
-            self.__arm_state_buffer = self.__arm_archer.get_simple_motor_status(
-            )
+            self.__arm_state_buffer = self.__arm.get_simple_motor_status()
 
         # (gripper_dofs, 3) # pos vel eff
         if self.__gripper is not None and self.__gripper_state_buffer is None:
@@ -220,24 +224,24 @@ class HexRobotHexarm(HexRobotBase):
         # [[pos_0, tor_0], ..., [pos_n, tor_n]]
         # cmds: (n, 5)
         # [[pos_0, vel_0, tor_0, kp_0, kd_0], ..., [pos_n, vel_n, tor_n, kp_n, kd_n]]
-        if self.__arm_archer is None:
+        if self.__arm is None:
             print("\033[91mArm not found\033[0m")
             return False
 
-        if cmds.shape[0] < self._dofs[0]:
+        if cmds.shape[0] < self._dofs_sum:
             print(
                 "\033[91mThe length of joint_angles must be greater than or equal to the number of motors\033[0m"
             )
             return False
-        elif cmds.shape[0] > self._dofs[0]:
+        elif cmds.shape[0] > self._dofs_sum:
             print(
                 f"\033[33mThe length of joint_angles is greater than the number of motors\033[0m"
             )
-            cmds = cmds[:self._dofs[0]]
+            cmds = cmds[:self._dofs_sum]
 
         cmd_pos = None
-        tar_vel = np.zeros(self._dofs[0])
-        cmd_tor = np.zeros(self._dofs[0])
+        tar_vel = np.zeros(self._dofs_sum)
+        cmd_tor = np.zeros(self._dofs_sum)
         cmd_kp = self.__mit_kp.copy()
         cmd_kd = self.__mit_kd.copy()
         if len(cmds.shape) == 1:
@@ -263,25 +267,25 @@ class HexRobotHexarm(HexRobotBase):
         )
 
         # arm
-        mit_cmd = self.__arm_archer.construct_mit_command(
-            tar_pos[:self.__arm_dofs],
-            tar_vel[:self.__arm_dofs],
-            cmd_tor[:self.__arm_dofs],
-            cmd_kp[:self.__arm_dofs],
-            cmd_kd[:self.__arm_dofs],
+        arm_cmd = self.__arm.construct_mit_command(
+            tar_pos[self.__motor_idx["robot_arm"]],
+            tar_vel[self.__motor_idx["robot_arm"]],
+            cmd_tor[self.__motor_idx["robot_arm"]],
+            cmd_kp[self.__motor_idx["robot_arm"]],
+            cmd_kd[self.__motor_idx["robot_arm"]],
         )
-        self.__arm_archer.motor_command(CommandType.MIT, mit_cmd)
+        self.__arm.motor_command(CommandType.MIT, arm_cmd)
 
         # gripper
         if self.__gripper is not None:
-            mit_cmd = self.__gripper.construct_mit_command(
-                tar_pos[self.__arm_dofs:],
-                tar_vel[self.__arm_dofs:],
-                cmd_tor[self.__arm_dofs:],
-                cmd_kp[self.__arm_dofs:],
-                cmd_kd[self.__arm_dofs:],
+            gripper_cmd = self.__gripper.construct_mit_command(
+                tar_pos[self.__motor_idx["robot_gripper"]],
+                tar_vel[self.__motor_idx["robot_gripper"]],
+                cmd_tor[self.__motor_idx["robot_gripper"]],
+                cmd_kp[self.__motor_idx["robot_gripper"]],
+                cmd_kd[self.__motor_idx["robot_gripper"]],
             )
-            self.__gripper.motor_command(CommandType.MIT, mit_cmd)
+            self.__gripper.motor_command(CommandType.MIT, gripper_cmd)
 
         return True
 
@@ -289,6 +293,6 @@ class HexRobotHexarm(HexRobotBase):
         if not self._working.is_set():
             return
         self._working.clear()
-        self.__arm_archer.stop()
+        self.__arm.stop()
         self.__hex_api.close()
         hex_log(HEX_LOG_LEVEL["info"], "HexRobotHexarm closed")

hex_zmq_servers/robot/hexarm/robot_hexarm_srv.py

@@ -36,9 +36,8 @@ ROBOT_CONFIG = {
     "device_port": 8439,
     "control_hz": 250,
     "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],
+    "mit_kp": [200.0, 200.0, 200.0, 75.0, 15.0, 15.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    "mit_kd": [12.5, 12.5, 12.5, 6.0, 0.31, 0.31, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
     "sens_ts": True,
 }