hex-zmq-servers 0.2.5__py3-none-any.whl → 0.3.1__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.
- hex_zmq_servers/__init__.py +20 -19
- hex_zmq_servers/config/robot_hexarm.json +1 -1
- hex_zmq_servers/hex_launch.py +178 -13
- hex_zmq_servers/mujoco/__init__.py +5 -5
- hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/__init__.py +6 -6
- hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/scene.xml +1 -1
- hex_zmq_servers/mujoco/{archer_d6y/mujoco_archer_d6y.py → archer_y6/mujoco_archer_y6.py} +4 -4
- hex_zmq_servers/mujoco/{archer_d6y/mujoco_archer_d6y_cli.py → archer_y6/mujoco_archer_y6_cli.py} +1 -1
- hex_zmq_servers/mujoco/{archer_d6y/mujoco_archer_d6y_srv.py → archer_y6/mujoco_archer_y6_srv.py} +5 -5
- hex_zmq_servers/mujoco/e3_desktop/mujoco_e3_desktop.py +2 -2
- hex_zmq_servers/robot/hexarm/__init__.py +10 -10
- hex_zmq_servers/robot/hexarm/robot_hexarm.py +2 -2
- hex_zmq_servers/robot/hexarm/urdf/{archer_d6y → archer_y6}/empty.urdf +15 -15
- hex_zmq_servers/robot/hexarm/urdf/{archer_d6y → archer_y6}/gp100.urdf +15 -15
- hex_zmq_servers/robot/hexarm/urdf/{archer_d6y → archer_y6}/gp100_handle.urdf +15 -15
- hex_zmq_servers/robot/hexarm/urdf/{archer_d6y → archer_y6}/gp100_p050.urdf +15 -15
- hex_zmq_servers/robot/hexarm/urdf/{archer_d6y → archer_y6}/gp100_p050_handle.urdf +15 -15
- {hex_zmq_servers-0.2.5.dist-info → hex_zmq_servers-0.3.1.dist-info}/METADATA +5 -4
- {hex_zmq_servers-0.2.5.dist-info → hex_zmq_servers-0.3.1.dist-info}/RECORD +41 -41
- /hex_zmq_servers/config/{mujoco_archer_d6y.json → mujoco_archer_y6.json} +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/arm_base_link.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/arm_link_1.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/arm_link_2.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/arm_link_3.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/arm_link_4.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/arm_link_5.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/assets.xml +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/camera_link.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/gripper_base_link.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/gripper_left_helper_link.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/gripper_left_link_1.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/gripper_left_link_2.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/gripper_right_helper_link.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/gripper_right_link_1.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/gripper_right_link_2.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/assets/table_link.STL +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/robot.xml +0 -0
- /hex_zmq_servers/mujoco/{archer_d6y → archer_y6}/model/setting.xml +0 -0
- {hex_zmq_servers-0.2.5.dist-info → hex_zmq_servers-0.3.1.dist-info}/WHEEL +0 -0
- {hex_zmq_servers-0.2.5.dist-info → hex_zmq_servers-0.3.1.dist-info}/licenses/LICENSE +0 -0
- {hex_zmq_servers-0.2.5.dist-info → hex_zmq_servers-0.3.1.dist-info}/top_level.txt +0 -0
hex_zmq_servers/__init__.py
CHANGED
|
@@ -6,7 +6,7 @@
|
|
|
6
6
|
# Date : 2025-09-15
|
|
7
7
|
################################################################
|
|
8
8
|
|
|
9
|
-
from .hex_launch import HexLaunch, HEX_LOG_LEVEL, hex_log, hex_err
|
|
9
|
+
from .hex_launch import HexLaunch, HexNodeConfig, HEX_LOG_LEVEL, hex_dict_str, hex_log, hex_err
|
|
10
10
|
|
|
11
11
|
from .device_base import HexDeviceBase
|
|
12
12
|
from .zmq_base import HexRate, hex_zmq_ts_now, hex_zmq_ts_delta_ms
|
|
@@ -45,7 +45,9 @@ __all__ = [
|
|
|
45
45
|
|
|
46
46
|
# launch
|
|
47
47
|
"HexLaunch",
|
|
48
|
+
"HexNodeConfig",
|
|
48
49
|
"HEX_LOG_LEVEL",
|
|
50
|
+
"hex_dict_str",
|
|
49
51
|
"hex_log",
|
|
50
52
|
"hex_err",
|
|
51
53
|
|
|
@@ -76,8 +78,15 @@ __all__ = [
|
|
|
76
78
|
"HexRobotHexarmServer",
|
|
77
79
|
]
|
|
78
80
|
|
|
81
|
+
# Check optional dependencies availability
|
|
82
|
+
from importlib.util import find_spec
|
|
83
|
+
|
|
84
|
+
_HAS_OPENCV = find_spec("cv2") is not None
|
|
85
|
+
_HAS_BERXEL = find_spec("berxel_py_wrapper") is not None
|
|
86
|
+
_HAS_MUJOCO = find_spec("mujoco") is not None
|
|
87
|
+
|
|
79
88
|
# Optional: camera
|
|
80
|
-
|
|
89
|
+
if _HAS_OPENCV and _HAS_BERXEL:
|
|
81
90
|
from .cam import HexCamBase, HexCamClientBase, HexCamServerBase
|
|
82
91
|
from .cam import HexCamDummy, HexCamDummyClient, HexCamDummyServer
|
|
83
92
|
from .cam import HexCamBerxel, HexCamBerxelClient, HexCamBerxelServer
|
|
@@ -105,25 +114,21 @@ try:
|
|
|
105
114
|
"HexCamBerxelClient",
|
|
106
115
|
"HexCamBerxelServer",
|
|
107
116
|
])
|
|
108
|
-
except ImportError:
|
|
109
|
-
# berxel_py_wrapper not installed
|
|
110
|
-
# Install with: pip install hex_zmq_servers[berxel] or pip install hex_zmq_servers[all]
|
|
111
|
-
pass
|
|
112
117
|
|
|
113
118
|
# Optional: mujoco
|
|
114
|
-
|
|
119
|
+
if _HAS_OPENCV and _HAS_MUJOCO:
|
|
115
120
|
from .mujoco import HexMujocoBase, HexMujocoClientBase, HexMujocoServerBase
|
|
116
|
-
from .mujoco import
|
|
121
|
+
from .mujoco import HexMujocoArcherY6, HexMujocoArcherY6Client, HexMujocoArcherY6Server
|
|
117
122
|
from .mujoco import HexMujocoE3Desktop, HexMujocoE3DesktopClient, HexMujocoE3DesktopServer
|
|
118
123
|
HEX_ZMQ_SERVERS_PATH_DICT.update({
|
|
119
|
-
"
|
|
120
|
-
f"{file_dir}/mujoco/
|
|
124
|
+
"mujoco_archer_y6":
|
|
125
|
+
f"{file_dir}/mujoco/archer_y6/mujoco_archer_y6_srv.py",
|
|
121
126
|
"mujoco_e3_desktop":
|
|
122
127
|
f"{file_dir}/mujoco/e3_desktop/mujoco_e3_desktop_srv.py",
|
|
123
128
|
})
|
|
124
129
|
HEX_ZMQ_CONFIGS_PATH_DICT.update({
|
|
125
|
-
"
|
|
126
|
-
f"{file_dir}/config/
|
|
130
|
+
"mujoco_archer_y6":
|
|
131
|
+
f"{file_dir}/config/mujoco_archer_y6.json",
|
|
127
132
|
"mujoco_e3_desktop":
|
|
128
133
|
f"{file_dir}/config/mujoco_e3_desktop.json",
|
|
129
134
|
})
|
|
@@ -132,16 +137,12 @@ try:
|
|
|
132
137
|
"HexMujocoBase",
|
|
133
138
|
"HexMujocoClientBase",
|
|
134
139
|
"HexMujocoServerBase",
|
|
135
|
-
"
|
|
136
|
-
"
|
|
137
|
-
"
|
|
140
|
+
"HexMujocoArcherY6",
|
|
141
|
+
"HexMujocoArcherY6Client",
|
|
142
|
+
"HexMujocoArcherY6Server",
|
|
138
143
|
"HexMujocoE3Desktop",
|
|
139
144
|
"HexMujocoE3DesktopClient",
|
|
140
145
|
"HexMujocoE3DesktopServer",
|
|
141
146
|
])
|
|
142
|
-
except ImportError:
|
|
143
|
-
# mujoco not installed
|
|
144
|
-
# Install with: pip install hex_zmq_servers[mujoco] or pip install hex_zmq_servers[all]
|
|
145
|
-
pass
|
|
146
147
|
|
|
147
148
|
# print("#### Thanks for using hex_zmq_servers :D ####")
|
hex_zmq_servers/hex_launch.py
CHANGED
|
@@ -6,13 +6,15 @@
|
|
|
6
6
|
# Date : 2025-09-25
|
|
7
7
|
################################################################
|
|
8
8
|
|
|
9
|
+
from __future__ import annotations
|
|
10
|
+
|
|
9
11
|
import json
|
|
10
12
|
import os, sys, signal, subprocess, threading, time
|
|
11
13
|
import termios
|
|
14
|
+
import importlib.util
|
|
12
15
|
|
|
13
16
|
from datetime import datetime
|
|
14
17
|
from pathlib import Path
|
|
15
|
-
from typing import Dict, List
|
|
16
18
|
|
|
17
19
|
HEX_LOG_LEVEL = {
|
|
18
20
|
"info": 0,
|
|
@@ -29,16 +31,187 @@ def hex_err(message):
|
|
|
29
31
|
print(message, file=sys.stderr)
|
|
30
32
|
|
|
31
33
|
|
|
34
|
+
def dict_update(dict_raw: dict, dict_new: dict, add_new: bool = False):
|
|
35
|
+
for key, value in dict_new.items():
|
|
36
|
+
if key in dict_raw:
|
|
37
|
+
if isinstance(dict_raw[key], dict) and isinstance(value, dict):
|
|
38
|
+
dict_update(dict_raw[key], value)
|
|
39
|
+
else:
|
|
40
|
+
dict_raw[key] = value
|
|
41
|
+
elif add_new:
|
|
42
|
+
dict_raw[key] = value
|
|
43
|
+
|
|
44
|
+
|
|
45
|
+
def hex_dict_str(dict_raw: dict, indent: int = 0) -> str:
|
|
46
|
+
print_str = ("\n" + "*" * 50 + "\n") if indent == 0 else ""
|
|
47
|
+
for key, value in dict_raw.items():
|
|
48
|
+
if isinstance(value, dict):
|
|
49
|
+
print_str += f"{' ' * indent * 4}{key}:\n{hex_dict_str(value, indent + 1)}"
|
|
50
|
+
else:
|
|
51
|
+
print_str += f"{' ' * indent * 4}{key}: {value}\n"
|
|
52
|
+
print_str += ("*" * 50) if indent == 0 else ""
|
|
53
|
+
return print_str
|
|
54
|
+
|
|
55
|
+
|
|
56
|
+
class HexNodeConfig():
|
|
57
|
+
|
|
58
|
+
def __init__(
|
|
59
|
+
self,
|
|
60
|
+
init_params: dict[str, dict] | list[dict] | HexNodeConfig = {},
|
|
61
|
+
):
|
|
62
|
+
len_init_params = len(init_params)
|
|
63
|
+
if isinstance(init_params, list):
|
|
64
|
+
self._cfgs_dict = {cfg["name"]: cfg for cfg in init_params}
|
|
65
|
+
elif isinstance(init_params, dict):
|
|
66
|
+
self._cfgs_dict = init_params
|
|
67
|
+
if len_init_params != len(self._cfgs_dict):
|
|
68
|
+
raise ValueError(f"Invalid init_params: {init_params}")
|
|
69
|
+
elif isinstance(init_params, HexNodeConfig):
|
|
70
|
+
self._cfgs_dict = init_params._cfgs_dict
|
|
71
|
+
else:
|
|
72
|
+
raise ValueError(f"Invalid init_params: {init_params}")
|
|
73
|
+
|
|
74
|
+
assert len(self._cfgs_dict
|
|
75
|
+
) == len_init_params, f"Invalid init_params: {init_params}"
|
|
76
|
+
|
|
77
|
+
def __len__(self) -> int:
|
|
78
|
+
return len(self._cfgs_dict)
|
|
79
|
+
|
|
80
|
+
def get_cfgs(self, use_list: bool = True) -> list[dict]:
|
|
81
|
+
if use_list:
|
|
82
|
+
return list(self._cfgs_dict.values())
|
|
83
|
+
else:
|
|
84
|
+
return self._cfgs_dict
|
|
85
|
+
|
|
86
|
+
def add_cfgs(
|
|
87
|
+
self,
|
|
88
|
+
node_cfgs: list[dict] | dict[str, dict] | HexNodeConfig,
|
|
89
|
+
):
|
|
90
|
+
new_cfgs = {}
|
|
91
|
+
if isinstance(node_cfgs, list):
|
|
92
|
+
new_cfgs = {cfg["name"]: cfg for cfg in node_cfgs}
|
|
93
|
+
elif isinstance(node_cfgs, dict):
|
|
94
|
+
new_cfgs = node_cfgs
|
|
95
|
+
elif isinstance(node_cfgs, HexNodeConfig):
|
|
96
|
+
new_cfgs = node_cfgs.get_cfgs(use_list=False)
|
|
97
|
+
else:
|
|
98
|
+
raise ValueError(f"Invalid node_cfgs: {node_cfgs}")
|
|
99
|
+
|
|
100
|
+
dict_update(self._cfgs_dict, new_cfgs, add_new=True)
|
|
101
|
+
|
|
102
|
+
def __str__(self) -> str:
|
|
103
|
+
print_str = f"[HexNodeConfig] Total {len(self._cfgs_dict)} nodes:\n"
|
|
104
|
+
for name in self._cfgs_dict.keys():
|
|
105
|
+
print_str += f" - {name}\n"
|
|
106
|
+
return print_str
|
|
107
|
+
|
|
108
|
+
@staticmethod
|
|
109
|
+
def parse_node_params_dict(
|
|
110
|
+
node_params_dict: dict,
|
|
111
|
+
node_default_params_dict: dict,
|
|
112
|
+
) -> HexNodeConfig:
|
|
113
|
+
node_dict = {}
|
|
114
|
+
for cur_name, cur_default_params in node_default_params_dict.items():
|
|
115
|
+
if cur_name in node_params_dict.keys():
|
|
116
|
+
dict_update(cur_default_params, node_params_dict[cur_name])
|
|
117
|
+
node_dict[cur_name] = cur_default_params
|
|
118
|
+
return HexNodeConfig(node_dict)
|
|
119
|
+
|
|
120
|
+
@staticmethod
|
|
121
|
+
def get_launch_params_cfgs(
|
|
122
|
+
launch_params_dict: dict,
|
|
123
|
+
launch_default_params_dict: dict,
|
|
124
|
+
launch_path_dict: dict,
|
|
125
|
+
) -> HexNodeConfig:
|
|
126
|
+
cfg_list = []
|
|
127
|
+
for launch_name, launch_path in launch_path_dict.items():
|
|
128
|
+
node_default_params_dict = launch_default_params_dict.get(
|
|
129
|
+
launch_name, {})
|
|
130
|
+
|
|
131
|
+
node_params_dict = {}
|
|
132
|
+
if launch_name in launch_params_dict.keys():
|
|
133
|
+
node_params_dict = launch_params_dict[launch_name]
|
|
134
|
+
else:
|
|
135
|
+
for node_name in node_default_params_dict.keys():
|
|
136
|
+
if node_name in launch_params_dict.keys():
|
|
137
|
+
node_params_dict[node_name] = launch_params_dict[
|
|
138
|
+
node_name]
|
|
139
|
+
|
|
140
|
+
launch_update_cfg = HexNodeConfig.parse_node_params_dict(
|
|
141
|
+
node_params_dict,
|
|
142
|
+
node_default_params_dict,
|
|
143
|
+
)
|
|
144
|
+
|
|
145
|
+
cfg_list.append(
|
|
146
|
+
HexNodeConfig.get_node_cfgs_from_launch(
|
|
147
|
+
launch_path,
|
|
148
|
+
launch_update_cfg,
|
|
149
|
+
))
|
|
150
|
+
|
|
151
|
+
final_cfg = HexNodeConfig()
|
|
152
|
+
for cfg in cfg_list:
|
|
153
|
+
# use name as key to make sure every node has a unique name
|
|
154
|
+
final_cfg.add_cfgs(cfg.get_cfgs(use_list=True))
|
|
155
|
+
print(f"final_cfg: {final_cfg}")
|
|
156
|
+
return final_cfg
|
|
157
|
+
|
|
158
|
+
@staticmethod
|
|
159
|
+
def get_node_cfgs_from_launch(
|
|
160
|
+
launch_path: str,
|
|
161
|
+
params: dict | HexNodeConfig = {},
|
|
162
|
+
) -> HexNodeConfig:
|
|
163
|
+
# normalize the path
|
|
164
|
+
launch_path = os.path.abspath(launch_path)
|
|
165
|
+
if not os.path.exists(launch_path):
|
|
166
|
+
raise FileNotFoundError(f"Launch file not found: {launch_path}")
|
|
167
|
+
|
|
168
|
+
# load the module dynamically
|
|
169
|
+
spec = importlib.util.spec_from_file_location("launch_module",
|
|
170
|
+
launch_path)
|
|
171
|
+
if spec is None or spec.loader is None:
|
|
172
|
+
raise ImportError(f"Failed to load module from: {launch_path}")
|
|
173
|
+
|
|
174
|
+
# load module
|
|
175
|
+
launch_module = importlib.util.module_from_spec(spec)
|
|
176
|
+
sys.modules["launch_module"] = launch_module
|
|
177
|
+
spec.loader.exec_module(launch_module)
|
|
178
|
+
|
|
179
|
+
# check if `get_node_cfgs` function exists
|
|
180
|
+
if not hasattr(launch_module, "get_node_cfgs"):
|
|
181
|
+
raise AttributeError(
|
|
182
|
+
f"Function 'get_node_cfgs' not found in {launch_path}")
|
|
183
|
+
|
|
184
|
+
# call `get_node_cfgs` function
|
|
185
|
+
get_node_cfgs_func = getattr(launch_module, "get_node_cfgs")
|
|
186
|
+
if isinstance(params, dict):
|
|
187
|
+
print("params is dict")
|
|
188
|
+
node_cfgs = get_node_cfgs_func(params)
|
|
189
|
+
elif isinstance(params, HexNodeConfig):
|
|
190
|
+
print("params is HexNodeConfig")
|
|
191
|
+
node_cfgs = get_node_cfgs_func(params.get_cfgs(use_list=False))
|
|
192
|
+
else:
|
|
193
|
+
raise ValueError(f"Invalid params: {params}")
|
|
194
|
+
return node_cfgs
|
|
195
|
+
|
|
196
|
+
|
|
32
197
|
class HexLaunch:
|
|
33
198
|
|
|
34
199
|
def __init__(
|
|
35
200
|
self,
|
|
36
|
-
node_cfgs:
|
|
201
|
+
node_cfgs: list[dict] | dict[str, dict] | HexNodeConfig,
|
|
37
202
|
log_dir: str = "logs",
|
|
38
203
|
min_level: int = HEX_LOG_LEVEL["warn"],
|
|
39
204
|
):
|
|
40
|
-
|
|
41
|
-
|
|
205
|
+
if isinstance(node_cfgs, list):
|
|
206
|
+
node_cfgs = HexNodeConfig(node_cfgs)
|
|
207
|
+
elif isinstance(node_cfgs, dict):
|
|
208
|
+
node_cfgs = HexNodeConfig(node_cfgs)
|
|
209
|
+
elif isinstance(node_cfgs, HexNodeConfig):
|
|
210
|
+
node_cfgs = node_cfgs
|
|
211
|
+
else:
|
|
212
|
+
raise ValueError(f"Invalid node_cfgs: {node_cfgs}")
|
|
213
|
+
self.__node_cfgs = node_cfgs.get_cfgs(use_list=True)
|
|
214
|
+
self.__state: dict[str, dict] = {}
|
|
42
215
|
self.__stop_event = threading.Event()
|
|
43
216
|
self.__log_dir = Path(log_dir)
|
|
44
217
|
self.__log_dir.mkdir(parents=True, exist_ok=True)
|
|
@@ -102,20 +275,12 @@ class HexLaunch:
|
|
|
102
275
|
with open(cfg_path, "r", encoding="utf-8") as f:
|
|
103
276
|
cfg_file = json.load(f)
|
|
104
277
|
cfg_obj.update(cfg_file)
|
|
105
|
-
|
|
278
|
+
dict_update(cfg_obj, node_cfg.get("cfg", {}))
|
|
106
279
|
cfg_str = json.dumps(cfg_obj)
|
|
107
280
|
|
|
108
281
|
# build cmd
|
|
109
282
|
return [python_exe, "-u", str(node_path)] + ["--cfg", cfg_str]
|
|
110
283
|
|
|
111
|
-
def __dict_update(self, dict_raw: dict, dict_new: dict):
|
|
112
|
-
for key, value in dict_new.items():
|
|
113
|
-
if key in dict_raw:
|
|
114
|
-
if isinstance(dict_raw[key], dict) and isinstance(value, dict):
|
|
115
|
-
self.__dict_update(dict_raw[key], value)
|
|
116
|
-
else:
|
|
117
|
-
dict_raw[key] = value
|
|
118
|
-
|
|
119
284
|
def __stream_printer(
|
|
120
285
|
self,
|
|
121
286
|
prefix: str,
|
|
@@ -7,7 +7,7 @@
|
|
|
7
7
|
################################################################
|
|
8
8
|
|
|
9
9
|
from .mujoco_base import HexMujocoBase, HexMujocoClientBase, HexMujocoServerBase
|
|
10
|
-
from .
|
|
10
|
+
from .archer_y6 import HexMujocoArcherY6, HexMujocoArcherY6Client, HexMujocoArcherY6Server
|
|
11
11
|
from .e3_desktop import HexMujocoE3Desktop, HexMujocoE3DesktopClient, HexMujocoE3DesktopServer
|
|
12
12
|
|
|
13
13
|
__all__ = [
|
|
@@ -16,10 +16,10 @@ __all__ = [
|
|
|
16
16
|
"HexMujocoClientBase",
|
|
17
17
|
"HexMujocoServerBase",
|
|
18
18
|
|
|
19
|
-
#
|
|
20
|
-
"
|
|
21
|
-
"
|
|
22
|
-
"
|
|
19
|
+
# archer_y6
|
|
20
|
+
"HexMujocoArcherY6",
|
|
21
|
+
"HexMujocoArcherY6Client",
|
|
22
|
+
"HexMujocoArcherY6Server",
|
|
23
23
|
|
|
24
24
|
# e3_desktop
|
|
25
25
|
"HexMujocoE3Desktop",
|
|
@@ -6,12 +6,12 @@
|
|
|
6
6
|
# Date : 2025-09-15
|
|
7
7
|
################################################################
|
|
8
8
|
|
|
9
|
-
from .
|
|
10
|
-
from .
|
|
11
|
-
from .
|
|
9
|
+
from .mujoco_archer_y6 import HexMujocoArcherY6
|
|
10
|
+
from .mujoco_archer_y6_cli import HexMujocoArcherY6Client
|
|
11
|
+
from .mujoco_archer_y6_srv import HexMujocoArcherY6Server
|
|
12
12
|
|
|
13
13
|
__all__ = [
|
|
14
|
-
"
|
|
15
|
-
"
|
|
16
|
-
"
|
|
14
|
+
"HexMujocoArcherY6",
|
|
15
|
+
"HexMujocoArcherY6Client",
|
|
16
|
+
"HexMujocoArcherY6Server",
|
|
17
17
|
]
|
|
@@ -36,7 +36,7 @@ MUJOCO_CONFIG = {
|
|
|
36
36
|
}
|
|
37
37
|
|
|
38
38
|
|
|
39
|
-
class
|
|
39
|
+
class HexMujocoArcherY6(HexMujocoBase):
|
|
40
40
|
|
|
41
41
|
def __init__(
|
|
42
42
|
self,
|
|
@@ -219,8 +219,8 @@ class HexMujocoArcherD6y(HexMujocoBase):
|
|
|
219
219
|
tau_cmds = None
|
|
220
220
|
if not self.__tau_ctrl:
|
|
221
221
|
cmd_pos = None
|
|
222
|
-
tar_vel = np.
|
|
223
|
-
cmd_tor = np.
|
|
222
|
+
tar_vel = np.zeros(cmds.shape[0])
|
|
223
|
+
cmd_tor = np.zeros(cmds.shape[0])
|
|
224
224
|
cmd_kp = self.__mit_kp.copy()
|
|
225
225
|
cmd_kd = self.__mit_kd.copy()
|
|
226
226
|
if len(cmds.shape) == 1:
|
|
@@ -286,4 +286,4 @@ class HexMujocoArcherD6y(HexMujocoBase):
|
|
|
286
286
|
self.__depth_cam.close()
|
|
287
287
|
if not self.__headless:
|
|
288
288
|
self.__viewer.close()
|
|
289
|
-
hex_log(HEX_LOG_LEVEL["info"], "
|
|
289
|
+
hex_log(HEX_LOG_LEVEL["info"], "HexMujocoArcherY6 closed")
|
hex_zmq_servers/mujoco/{archer_d6y/mujoco_archer_d6y_srv.py → archer_y6/mujoco_archer_y6_srv.py}
RENAMED
|
@@ -11,7 +11,7 @@ from hex_zmq_servers.zmq_base import HexSafeValue
|
|
|
11
11
|
|
|
12
12
|
try:
|
|
13
13
|
from ..mujoco_base import HexMujocoServerBase
|
|
14
|
-
from .
|
|
14
|
+
from .mujoco_archer_y6 import HexMujocoArcherY6
|
|
15
15
|
except (ImportError, ValueError):
|
|
16
16
|
import sys
|
|
17
17
|
from pathlib import Path
|
|
@@ -20,7 +20,7 @@ except (ImportError, ValueError):
|
|
|
20
20
|
if str(project_root) not in sys.path:
|
|
21
21
|
sys.path.insert(0, str(project_root))
|
|
22
22
|
from hex_zmq_servers.mujoco.mujoco_base import HexMujocoServerBase
|
|
23
|
-
from hex_zmq_servers.mujoco.
|
|
23
|
+
from hex_zmq_servers.mujoco.archer_y6.mujoco_archer_y6 import HexMujocoArcherY6
|
|
24
24
|
|
|
25
25
|
NET_CONFIG = {
|
|
26
26
|
"ip": "127.0.0.1",
|
|
@@ -38,7 +38,7 @@ MUJOCO_CONFIG = {
|
|
|
38
38
|
}
|
|
39
39
|
|
|
40
40
|
|
|
41
|
-
class
|
|
41
|
+
class HexMujocoArcherY6Server(HexMujocoServerBase):
|
|
42
42
|
|
|
43
43
|
def __init__(
|
|
44
44
|
self,
|
|
@@ -48,7 +48,7 @@ class HexMujocoArcherD6yServer(HexMujocoServerBase):
|
|
|
48
48
|
HexMujocoServerBase.__init__(self, net_config)
|
|
49
49
|
|
|
50
50
|
# mujoco
|
|
51
|
-
self._device =
|
|
51
|
+
self._device = HexMujocoArcherY6(params_config)
|
|
52
52
|
|
|
53
53
|
# values
|
|
54
54
|
self._states_robot_value = HexSafeValue()
|
|
@@ -139,4 +139,4 @@ if __name__ == "__main__":
|
|
|
139
139
|
args = parser.parse_args()
|
|
140
140
|
cfg = json.loads(args.cfg)
|
|
141
141
|
|
|
142
|
-
hex_server_helper(cfg,
|
|
142
|
+
hex_server_helper(cfg, HexMujocoArcherY6Server)
|
|
@@ -305,8 +305,8 @@ class HexMujocoE3Desktop(HexMujocoBase):
|
|
|
305
305
|
tau_cmds = None
|
|
306
306
|
if not self.__tau_ctrl:
|
|
307
307
|
cmd_pos = None
|
|
308
|
-
tar_vel = np.
|
|
309
|
-
cmd_tor = np.
|
|
308
|
+
tar_vel = np.zeros(cmds.shape[0])
|
|
309
|
+
cmd_tor = np.zeros(cmds.shape[0])
|
|
310
310
|
cmd_kp = self.__mit_kp.copy()
|
|
311
311
|
cmd_kd = self.__mit_kd.copy()
|
|
312
312
|
if len(cmds.shape) == 1:
|
|
@@ -14,16 +14,16 @@ import os
|
|
|
14
14
|
|
|
15
15
|
urdf_dir = os.path.join(os.path.dirname(__file__), "urdf")
|
|
16
16
|
HEXARM_URDF_PATH_DICT = {
|
|
17
|
-
"
|
|
18
|
-
f"{urdf_dir}/
|
|
19
|
-
"
|
|
20
|
-
f"{urdf_dir}/
|
|
21
|
-
"
|
|
22
|
-
f"{urdf_dir}/
|
|
23
|
-
"
|
|
24
|
-
f"{urdf_dir}/
|
|
25
|
-
"
|
|
26
|
-
f"{urdf_dir}/
|
|
17
|
+
"archer_y6_empty":
|
|
18
|
+
f"{urdf_dir}/archer_y6/empty.urdf",
|
|
19
|
+
"archer_y6_gp100":
|
|
20
|
+
f"{urdf_dir}/archer_y6/gp100.urdf",
|
|
21
|
+
"archer_y6_gp100_handle":
|
|
22
|
+
f"{urdf_dir}/archer_y6/gp100_handle.urdf",
|
|
23
|
+
"archer_y6_gp100_p050":
|
|
24
|
+
f"{urdf_dir}/archer_y6/gp100_p050.urdf",
|
|
25
|
+
"archer_y6_gp100_p050_handle":
|
|
26
|
+
f"{urdf_dir}/archer_y6/gp100_p050_handle.urdf",
|
|
27
27
|
"archer_l6y_empty":
|
|
28
28
|
f"{urdf_dir}/archer_l6y/empty.urdf",
|
|
29
29
|
"archer_l6y_gp100":
|
|
@@ -25,7 +25,7 @@ ROBOT_CONFIG = {
|
|
|
25
25
|
"device_ip": "172.18.8.161",
|
|
26
26
|
"device_port": 8439,
|
|
27
27
|
"control_hz": 250,
|
|
28
|
-
"arm_type": "
|
|
28
|
+
"arm_type": "archer_y6",
|
|
29
29
|
"use_gripper": True,
|
|
30
30
|
"mit_kp": [200.0, 200.0, 200.0, 75.0, 15.0, 15.0, 20.0],
|
|
31
31
|
"mit_kd": [12.5, 12.5, 12.5, 6.0, 0.31, 0.31, 1.0],
|
|
@@ -33,7 +33,7 @@ ROBOT_CONFIG = {
|
|
|
33
33
|
}
|
|
34
34
|
|
|
35
35
|
HEX_DEVICE_TYPE_DICT = {
|
|
36
|
-
"
|
|
36
|
+
"archer_y6": 16,
|
|
37
37
|
"archer_l6y": 17,
|
|
38
38
|
}
|
|
39
39
|
|
|
@@ -1,5 +1,5 @@
|
|
|
1
1
|
<?xml version="1.0" encoding="utf-8"?>
|
|
2
|
-
<robot name="
|
|
2
|
+
<robot name="archer_y6">
|
|
3
3
|
|
|
4
4
|
<link name="base_link">
|
|
5
5
|
<inertial>
|
|
@@ -10,7 +10,7 @@
|
|
|
10
10
|
<visual>
|
|
11
11
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
12
12
|
<geometry>
|
|
13
|
-
<mesh filename="package://
|
|
13
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/base_link.STL" />
|
|
14
14
|
</geometry>
|
|
15
15
|
<material name="">
|
|
16
16
|
<color rgba="1 1 1 1" />
|
|
@@ -19,7 +19,7 @@
|
|
|
19
19
|
<collision>
|
|
20
20
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
21
21
|
<geometry>
|
|
22
|
-
<mesh filename="package://
|
|
22
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/base_link.STL" />
|
|
23
23
|
</geometry>
|
|
24
24
|
</collision>
|
|
25
25
|
</link>
|
|
@@ -33,7 +33,7 @@
|
|
|
33
33
|
<visual>
|
|
34
34
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
35
35
|
<geometry>
|
|
36
|
-
<mesh filename="package://
|
|
36
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_1.STL" />
|
|
37
37
|
</geometry>
|
|
38
38
|
<material name="">
|
|
39
39
|
<color rgba="1 1 1 1" />
|
|
@@ -42,7 +42,7 @@
|
|
|
42
42
|
<collision>
|
|
43
43
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
44
44
|
<geometry>
|
|
45
|
-
<mesh filename="package://
|
|
45
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_1.STL" />
|
|
46
46
|
</geometry>
|
|
47
47
|
</collision>
|
|
48
48
|
</link>
|
|
@@ -63,7 +63,7 @@
|
|
|
63
63
|
<visual>
|
|
64
64
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
65
65
|
<geometry>
|
|
66
|
-
<mesh filename="package://
|
|
66
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_2.STL" />
|
|
67
67
|
</geometry>
|
|
68
68
|
<material name="">
|
|
69
69
|
<color rgba="1 1 1 1" />
|
|
@@ -72,7 +72,7 @@
|
|
|
72
72
|
<collision>
|
|
73
73
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
74
74
|
<geometry>
|
|
75
|
-
<mesh filename="package://
|
|
75
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_2.STL" />
|
|
76
76
|
</geometry>
|
|
77
77
|
</collision>
|
|
78
78
|
</link>
|
|
@@ -93,7 +93,7 @@
|
|
|
93
93
|
<visual>
|
|
94
94
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
95
95
|
<geometry>
|
|
96
|
-
<mesh filename="package://
|
|
96
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_3.STL" />
|
|
97
97
|
</geometry>
|
|
98
98
|
<material name="">
|
|
99
99
|
<color rgba="1 1 1 1" />
|
|
@@ -102,7 +102,7 @@
|
|
|
102
102
|
<collision>
|
|
103
103
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
104
104
|
<geometry>
|
|
105
|
-
<mesh filename="package://
|
|
105
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_3.STL" />
|
|
106
106
|
</geometry>
|
|
107
107
|
</collision>
|
|
108
108
|
</link>
|
|
@@ -123,7 +123,7 @@
|
|
|
123
123
|
<visual>
|
|
124
124
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
125
125
|
<geometry>
|
|
126
|
-
<mesh filename="package://
|
|
126
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_4.STL" />
|
|
127
127
|
</geometry>
|
|
128
128
|
<material name="">
|
|
129
129
|
<color rgba="1 1 1 1" />
|
|
@@ -132,7 +132,7 @@
|
|
|
132
132
|
<collision>
|
|
133
133
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
134
134
|
<geometry>
|
|
135
|
-
<mesh filename="package://
|
|
135
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_4.STL" />
|
|
136
136
|
</geometry>
|
|
137
137
|
</collision>
|
|
138
138
|
</link>
|
|
@@ -153,7 +153,7 @@
|
|
|
153
153
|
<visual>
|
|
154
154
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
155
155
|
<geometry>
|
|
156
|
-
<mesh filename="package://
|
|
156
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_5.STL" />
|
|
157
157
|
</geometry>
|
|
158
158
|
<material name="">
|
|
159
159
|
<color rgba="1 1 1 1" />
|
|
@@ -162,7 +162,7 @@
|
|
|
162
162
|
<collision>
|
|
163
163
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
164
164
|
<geometry>
|
|
165
|
-
<mesh filename="package://
|
|
165
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_5.STL" />
|
|
166
166
|
</geometry>
|
|
167
167
|
</collision>
|
|
168
168
|
</link>
|
|
@@ -183,7 +183,7 @@
|
|
|
183
183
|
<visual>
|
|
184
184
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
185
185
|
<geometry>
|
|
186
|
-
<mesh filename="package://
|
|
186
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_6.STL" />
|
|
187
187
|
</geometry>
|
|
188
188
|
<material name="">
|
|
189
189
|
<color rgba="1 1 1 1" />
|
|
@@ -192,7 +192,7 @@
|
|
|
192
192
|
<collision>
|
|
193
193
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
194
194
|
<geometry>
|
|
195
|
-
<mesh filename="package://
|
|
195
|
+
<mesh filename="package://xpkg_urdf_archer_y6/meshes/link_6.STL" />
|
|
196
196
|
</geometry>
|
|
197
197
|
</collision>
|
|
198
198
|
</link>
|