diff --git a/cobot-setting.yaml b/cobot-setting.yaml index ec55b12..a2aef58 100644 --- a/cobot-setting.yaml +++ b/cobot-setting.yaml @@ -18,7 +18,7 @@ digital_twin: controller_timer: "50" cameras: - - pkg://iiwa_config/config/cameras/d455_top.yaml + - pkg://iiwa_config/config/cameras/d455_top.yaml rviz: config: pkg://iiwa_config/config/rviz/rviz_moveit.rviz @@ -36,6 +36,9 @@ controller: moveit_cpp: pkg://iiwa_config/config/moveit/moveit_cpp.yaml +tool: + active: "patron" # Активный инструмент: none | patron | ... (из tools.yaml) + planning: pose_link: "tcp" # TCP-линк для декартовых целей planning_group: "iiwa_arm" # Группа планирования из SRDF @@ -68,12 +71,12 @@ foxglove: send_buffer_limit: 10000000 # Максимальный размер буфера отправки в байтах (защита от OOM при медленном клиенте) use_sim_time: false # Использовать симуляционное время /clock вместо системного capabilities: # Список возможностей, открытых клиенту - - clientPublish - - parameters - - parametersSubscribe - - services - - connectionGraph - - assets + - clientPublish + - parameters + - parametersSubscribe + - services + - connectionGraph + - assets include_hidden: false # Показывать клиенту скрытые топики и сервисы (начинаются с _) asset_uri_allowlist: ['^package://(?:[-\w%]+/)*[-\w%.]+\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$'] # Regex-список URI вида package://..., из которых bridge разрешает отдавать файлы-ассеты (URDF, mesh и т.п.) - ignore_unresponsive_param_nodes: true # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте) \ No newline at end of file + ignore_unresponsive_param_nodes: true # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте) diff --git a/cobot/commands/robot_setup.py b/cobot/commands/robot_setup.py index efabf3d..2e7ca6f 100644 --- a/cobot/commands/robot_setup.py +++ b/cobot/commands/robot_setup.py @@ -17,6 +17,12 @@ from cobot.tui import SCREEN_CSS, InputScreen, PickScreen _PROJECT_DIR = Path(__file__).parent.parent.parent _CONFIG_PATH = _PROJECT_DIR / "cobot-setting.yaml" +_TOOLS_YAML = _PROJECT_DIR / "src" / "iiwa_config" / "config" / "tools.yaml" +_TOOL_ACTIVE_XACRO = ( + _PROJECT_DIR / "src" / "iiwa_description" / "urdf" / "tools" / "tool_active.xacro" +) +_SRDF_PATH = _PROJECT_DIR / "src" / "iiwa_config" / "config" / "moveit" / "iiwa7.srdf" + # Use ruamel.yaml instead of PyYAML so comments and formatting in the config file are preserved. # Используем ruamel.yaml вместо PyYAML, чтобы комментарии и форматирование в конфиге сохранялись. _yaml = YAML() @@ -48,6 +54,41 @@ class _Block: fields: List[_Field] +def _load_tools_registry() -> dict: + """Читает tools.yaml и возвращает словарь инструментов.""" + if not _TOOLS_YAML.exists(): + return {} + _y = YAML() + with open(_TOOLS_YAML, encoding="utf-8") as f: + data = _y.load(f) + return dict(data.get("tools", {})) + + +def _build_tool_block() -> Optional[_Block]: + """Строит блок выбора инструмента из реестра tools.yaml. + Возвращает None если реестр недоступен.""" + registry = _load_tools_registry() + if not registry: + return None + options = list(registry.keys()) + labels = " | ".join( + f"{name}: {registry[name].get('label', '')}" for name in options + ) + return _Block( + yaml_key="tool", + title="Tool / End-effector", + fields=[ + _Field( + "active", + "Выберите активный инструмент:", + options[0], + note=labels, + options=options, + ), + ], + ) + + # All configuration blocks. Each block maps to a top-level key in cobot-setting.yaml. # Все блоки конфигурации. Каждый блок соответствует ключу верхнего уровня в cobot-setting.yaml. _BLOCKS: List[_Block] = [ @@ -75,8 +116,6 @@ _BLOCKS: List[_Block] = [ yaml_key="planning", title="MoveIt planning", fields=[ - _Field("pose_link", "TCP link name:", "tcp", - note="Link used as the end-effector for Cartesian goals (defined in URDF/SRDF)"), _Field("planning_group", "Planning group:", "iiwa_arm", note="MoveIt planning group as defined in the SRDF"), _Field("default_frame", "Default reference frame:", "base_link"), @@ -204,14 +243,15 @@ class _Wizard(App[None]): """ CSS = SCREEN_CSS - def __init__(self, data: Any): + def __init__(self, data: Any, extra_blocks: Optional[List[_Block]] = None): super().__init__() self._data = data - self._blocks = list(_BLOCKS) + self._blocks = list(extra_blocks or []) + list(_BLOCKS) self._block_idx = 0 self._field_idx = 0 self._current_block: Optional[_Block] = None self._pending_fields: List[_Field] = [] + self.did_save = False def on_mount(self) -> None: self._next_block() @@ -221,6 +261,7 @@ class _Wizard(App[None]): # All blocks done - save and show the confirmation screen. # Все блоки пройдены - сохраняем и показываем экран подтверждения. _save_config(self._data) + self.did_save = True self.push_screen(_SavedScreen(), lambda _: self.exit()) return block = self._blocks[self._block_idx] @@ -317,10 +358,60 @@ def register(subparsers: argparse._SubParsersAction) -> None: def run(args: argparse.Namespace) -> None: + from rich.console import Console + console = Console() + if not _CONFIG_PATH.exists(): - from rich.console import Console - Console().print(f"[red]Config not found:[/red] {_CONFIG_PATH}") + console.print(f"[red]Config not found:[/red] {_CONFIG_PATH}") sys.exit(1) data = _load_config() - _Wizard(data).run() + + # Убеждаемся что секция tool существует в данных (для старых конфигов) + if "tool" not in data: + data["tool"] = {"active": "patron"} + + tool_block = _build_tool_block() + extra = [tool_block] if tool_block else [] + + wizard = _Wizard(data, extra_blocks=extra) + wizard.run() + + if not wizard.did_save: + return + + # Применяем выбранный инструмент: перезаписываем tool_active.xacro и iiwa7.srdf + active_tool = str(data["tool"].get("active", "patron")) + if _TOOLS_YAML.exists(): + try: + registry = _load_tools_registry() + if active_tool not in registry: + raise ValueError( + f"Unknown tool '{active_tool}'. Available: {', '.join(registry)}" + ) + tool_cfg = dict(registry[active_tool]) + + import sys as _sys + _sys.path.insert(0, str(_PROJECT_DIR / "src" / "iiwa_utils")) + from iiwa_utils.tool_manager import apply_tool + apply_tool( + tool_cfg=tool_cfg, + xacro_out_path=_TOOL_ACTIVE_XACRO, + srdf_path=_SRDF_PATH, + ) + + # Синхронизируем planning.pose_link с tcp_link выбранного инструмента + tcp_link = tool_cfg.get("tcp_link", "link_ee") + if "planning" in data: + data["planning"]["pose_link"] = tcp_link + _save_config(data) + + console.print( + f"[green]✓[/green] Tool [bold]{active_tool}[/bold] applied: " + f"tool_active.xacro, iiwa7.srdf updated, " + f"planning.pose_link → [bold]{tcp_link}[/bold]." + ) + except Exception as exc: + console.print(f"[red]Tool apply failed:[/red] {exc}") + else: + console.print(f"[yellow]tools.yaml not found at {_TOOLS_YAML} — skipping tool apply[/yellow]") diff --git a/src/iiwa_config/config/moveit/iiwa7.srdf b/src/iiwa_config/config/moveit/iiwa7.srdf index ee69f82..5379f3c 100644 --- a/src/iiwa_config/config/moveit/iiwa7.srdf +++ b/src/iiwa_config/config/moveit/iiwa7.srdf @@ -1,32 +1,11 @@ - + - - - - - + - - + @@ -54,9 +33,9 @@ - + - + @@ -67,26 +46,27 @@ - - - - - + + + + + + @@ -119,4 +99,4 @@ - \ No newline at end of file + diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index ec55b12..2352374 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -36,6 +36,9 @@ controller: moveit_cpp: pkg://iiwa_config/config/moveit/moveit_cpp.yaml +tool: + active: "patron" # Активный инструмент: none | patron | ... (из tools.yaml) + planning: pose_link: "tcp" # TCP-линк для декартовых целей planning_group: "iiwa_arm" # Группа планирования из SRDF diff --git a/src/iiwa_config/config/tools.yaml b/src/iiwa_config/config/tools.yaml new file mode 100644 index 0000000..8f3e014 --- /dev/null +++ b/src/iiwa_config/config/tools.yaml @@ -0,0 +1,64 @@ +# Реестр инструментов / захватов для KUKA iiwa7. +# +# Поля каждого инструмента: +# label — название, отображаемое в меню robot-setup +# xacro — путь до xacro-файла инструмента (xacro $(find ...) синтаксис); +# null = без захвата (голый фланец) +# tip_link — конец кинематической цепочки в SRDF () +# tcp_link — фрейм TCP для Декартовых целей (pose_link в planning) +# collisions — пары disable_collisions специфичные для этого инструмента; +# базовые пары робота (link1..link7) добавляются автоматически + +tools: + + none: + label: "Без захвата" + xacro: null + tip_link: "link_ee" + tcp_link: "link_ee" + collisions: [] + + patron: + label: "patron" + xacro: "$(find iiwa_description)/urdf/tools/patron.xacro" + tip_link: "patron" + tcp_link: "tcp" + collisions: + - [link1, patron, Never] + - [link2, patron, Never] + - [link3, patron, Never] + - [link4, patron, Never] + - [link5, patron, Never] + - [link6, patron, Never] + - [link7, patron, Adjacent] + - [link7, camera_holder, Adjacent] + - [link6, camera_holder, Never] + - [link5, camera_holder, Never] + - [link4, camera_holder, Never] + - [link3, camera_holder, Never] + - [link2, camera_holder, Never] + - [link1, camera_holder, Never] + - [base_link, camera_holder, Never] + - [camera_holder, patron, Adjacent] + - [camera_holder, camera_corner, Adjacent] + - [camera_corner, patron, Never] + - [camera_corner, link7, Never] + - [camera_corner, link6, Never] + - [camera_corner, link5, Never] + - [camera_corner, link4, Never] + - [camera_corner, link3, Never] + - [camera_corner, link2, Never] + - [camera_corner, link1, Never] + - [camera_corner, base_link, Never] + - [camera_corner, camera_hand, Adjacent] + - [camera_hand, camera_holder, Never] + - [camera_hand, patron, Never] + - [camera_hand, link7, Never] + - [camera_hand, link6, Never] + - [camera_hand, link5, Never] + - [camera_hand, link4, Never] + - [camera_hand, link3, Never] + - [camera_hand, link2, Never] + - [camera_hand, link1, Never] + - [camera_hand, base_link, Never] + diff --git a/src/iiwa_description/urdf/gripper/___gripper.urdf b/src/iiwa_description/urdf/gripper/___gripper.urdf deleted file mode 100644 index e8d5c08..0000000 --- a/src/iiwa_description/urdf/gripper/___gripper.urdf +++ /dev/null @@ -1,176 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/src/iiwa_description/urdf/gripper/gripper.xacro b/src/iiwa_description/urdf/gripper/gripper.xacro deleted file mode 100644 index 1bcfc8c..0000000 --- a/src/iiwa_description/urdf/gripper/gripper.xacro +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/iiwa_description/urdf/gripper/gripper_joints.xacro b/src/iiwa_description/urdf/gripper/gripper_joints.xacro deleted file mode 100644 index d7063de..0000000 --- a/src/iiwa_description/urdf/gripper/gripper_joints.xacro +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/iiwa_description/urdf/gripper/gripper_links.xacro b/src/iiwa_description/urdf/gripper/gripper_links.xacro deleted file mode 100644 index 630b8a6..0000000 --- a/src/iiwa_description/urdf/gripper/gripper_links.xacro +++ /dev/null @@ -1,67 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/iiwa_description/urdf/gripper/gripper_macros.xacro b/src/iiwa_description/urdf/gripper/gripper_macros.xacro deleted file mode 100644 index 920aeb0..0000000 --- a/src/iiwa_description/urdf/gripper/gripper_macros.xacro +++ /dev/null @@ -1,65 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/src/iiwa_description/urdf/gripper/gripper_meshes.xacro b/src/iiwa_description/urdf/gripper/gripper_meshes.xacro deleted file mode 100644 index 194fce7..0000000 --- a/src/iiwa_description/urdf/gripper/gripper_meshes.xacro +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - \ No newline at end of file diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index e9f58e9..917ee7b 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -17,7 +17,7 @@ - + diff --git a/src/iiwa_description/urdf/tools/tool_active.xacro b/src/iiwa_description/urdf/tools/tool_active.xacro new file mode 100644 index 0000000..1bd4e22 --- /dev/null +++ b/src/iiwa_description/urdf/tools/tool_active.xacro @@ -0,0 +1,5 @@ + + + + + diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index 5e6b58d..0161ab4 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/src/iiwa_utils/iiwa_utils/setting_loader.py @@ -65,6 +65,11 @@ class PlanningCfg: planning_attempts: int +@dataclass(frozen=True) +class ToolCfg: + active: str # ключ из tools.yaml: "none" | "patron" | ... + + @dataclass(frozen=True) class WebCfg: enabled: bool @@ -108,6 +113,7 @@ class Settings: digital_twin: DigitalTwinCfg controller: ControllerCfg planning: PlanningCfg + tool: ToolCfg foxglove: FoxgloveCfg web: WebCfg @@ -336,6 +342,12 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings: planning_attempts=int(planning_raw.get("planning_attempts", 3)), ) + # tool + tool_raw = raw.get("tool", {}) + tool = ToolCfg( + active=str(tool_raw.get("active", "patron")), + ) + # foxglove foxglove = _parse_foxglove(raw.get("foxglove")) @@ -347,6 +359,7 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings: digital_twin=digital_twin, controller=controller, planning=planning, + tool=tool, foxglove=foxglove, web=web, ) diff --git a/src/iiwa_utils/iiwa_utils/tool_manager.py b/src/iiwa_utils/iiwa_utils/tool_manager.py new file mode 100644 index 0000000..f70bbd9 --- /dev/null +++ b/src/iiwa_utils/iiwa_utils/tool_manager.py @@ -0,0 +1,157 @@ +""" +Управление активным инструментом робота. + +Применяет выбранный инструмент из реестра tools.yaml: + 1. Перезаписывает tool_active.xacro — URDF подхватывает его при следующем запуске. + 2. Перегенерирует iiwa7.srdf — сохраняет текущие group_state, обновляет + кинематическую цепочку, end_effector и пары disable_collisions. +""" + +from __future__ import annotations + +import xml.etree.ElementTree as ET +from pathlib import Path +from typing import Any + + +# --------------------------------------------------------------------------- +# Базовые пары столкновений робота (не зависят от инструмента). +# --------------------------------------------------------------------------- +_BASE_COLLISIONS: list[tuple[str, str, str]] = [ + ("base_link", "link1", "Adjacent"), + ("base_link", "link2", "Never"), + ("base_link", "link3", "Never"), + ("base_link", "link4", "Never"), + ("link1", "link2", "Adjacent"), + ("link1", "link3", "Never"), + ("link1", "link4", "Never"), + ("link1", "link5", "Never"), + ("link1", "link6", "Never"), + ("link1", "link7", "Never"), + ("link2", "link3", "Adjacent"), + ("link2", "link4", "Never"), + ("link2", "link5", "Never"), + ("link2", "link6", "Never"), + ("link2", "link7", "Never"), + ("link3", "link4", "Adjacent"), + ("link3", "link5", "Never"), + ("link3", "link6", "Never"), + ("link3", "link7", "Never"), + ("link4", "link5", "Adjacent"), + ("link4", "link6", "Never"), + ("link4", "link7", "Never"), + ("link5", "link6", "Adjacent"), + ("link5", "link7", "Never"), + ("link6", "link7", "Adjacent"), +] + + +def load_registry(tools_yaml_path: Path) -> dict[str, Any]: + """Загружает реестр инструментов из tools.yaml. + Поддерживает PyYAML и ruamel.yaml (что доступно в окружении).""" + try: + import yaml as _yaml + with open(tools_yaml_path, encoding="utf-8") as f: + data = _yaml.safe_load(f) + except ImportError: + from ruamel.yaml import YAML as _RYAML + _y = _RYAML() + with open(tools_yaml_path, encoding="utf-8") as f: + data = _y.load(f) + return dict(data.get("tools", {})) + + +def _read_group_states(srdf_path: Path) -> list[dict]: + """Читает group_state из существующего SRDF, чтобы сохранить их при регенерации.""" + if not srdf_path.exists(): + return [] + tree = ET.parse(srdf_path) + root = tree.getroot() + states = [] + for gs in root.findall("group_state"): + joints = {j.get("name"): j.get("value") for j in gs.findall("joint")} + states.append({ + "name": gs.get("name"), + "group": gs.get("group"), + "joints": joints, + }) + return states + + +def _write_xacro(tool_cfg: dict, output_path: Path) -> None: + """Записывает tool_active.xacro с include нужного инструмента (или пустой).""" + xacro_include = tool_cfg.get("xacro") + if xacro_include: + body = f' \n' + else: + body = " \n" + + content = ( + '\n' + '\n' + '\n' + + body + + '\n' + ) + output_path.write_text(content, encoding="utf-8") + + +def _write_srdf( + tool_cfg: dict, + group_states: list[dict], + output_path: Path, +) -> None: + """Генерирует iiwa7.srdf с учётом выбранного инструмента.""" + tip_link = tool_cfg.get("tip_link", "link_ee") + tool_collisions = [tuple(c) for c in tool_cfg.get("collisions", [])] + + lines: list[str] = [ + '', + '', + '', + '', + ' ', + f' ', + ' ', + ] + + if group_states: + lines.append('') + for gs in group_states: + lines.append(f' ') + for jname, jval in gs["joints"].items(): + lines.append(f' ') + lines.append(' ') + + if tip_link != "link_ee": + lines += [ + '', + f' ', + ] + + lines.append('') + for l1, l2, reason in _BASE_COLLISIONS: + lines.append(f' ') + + if tool_collisions: + lines.append('') + for l1, l2, reason in tool_collisions: + lines.append(f' ') + + lines += ['', ''] + output_path.write_text('\n'.join(lines), encoding="utf-8") + + +def apply_tool( + tool_cfg: dict, + xacro_out_path: Path, + srdf_path: Path, +) -> None: + """ + Применяет инструмент по его конфигу из реестра: + - перезаписывает xacro_out_path (tool_active.xacro) + - регенерирует srdf_path (iiwa7.srdf), сохраняя group_state + """ + group_states = _read_group_states(srdf_path) + _write_xacro(tool_cfg, xacro_out_path) + _write_srdf(tool_cfg, group_states, srdf_path)