Refactor tool management and update robot configuration
- Updated iiwa7.srdf to streamline group and end effector definitions, removing unnecessary joint specifications. - Enhanced setting.yaml to include active tool configuration. - Removed deprecated gripper URDF and Xacro files, consolidating tool definitions into tools.yaml. - Introduced tool_manager.py for dynamic tool management, allowing for easy updates to active tools and collision settings. - Created tool_active.xacro to reflect the currently active tool in the robot's URDF. - Added comprehensive collision management for the new tool configurations.
This commit is contained in:
Даниил Грабарь
@@ -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,
|
||||
)
|
||||
|
||||
@@ -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' <xacro:include filename="{xacro_include}"/>\n'
|
||||
else:
|
||||
body = " <!-- no tool attached -->\n"
|
||||
|
||||
content = (
|
||||
'<?xml version="1.0"?>\n'
|
||||
'<!-- AUTO-GENERATED by cobot robot-setup — do not edit manually -->\n'
|
||||
'<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="tool_active">\n'
|
||||
+ body +
|
||||
'</robot>\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] = [
|
||||
'<?xml version="1.0" encoding="UTF-8"?>',
|
||||
'<!-- AUTO-GENERATED by cobot robot-setup — do not edit manually -->',
|
||||
'<robot name="iiwa7">',
|
||||
'',
|
||||
' <group name="iiwa_arm">',
|
||||
f' <chain base_link="base_link" tip_link="{tip_link}"/>',
|
||||
' </group>',
|
||||
]
|
||||
|
||||
if group_states:
|
||||
lines.append('')
|
||||
for gs in group_states:
|
||||
lines.append(f' <group_state name="{gs["name"]}" group="{gs["group"]}">')
|
||||
for jname, jval in gs["joints"].items():
|
||||
lines.append(f' <joint name="{jname}" value="{jval}"/>')
|
||||
lines.append(' </group_state>')
|
||||
|
||||
if tip_link != "link_ee":
|
||||
lines += [
|
||||
'',
|
||||
f' <end_effector name="{tip_link}" parent_link="{tip_link}" group="iiwa_arm"/>',
|
||||
]
|
||||
|
||||
lines.append('')
|
||||
for l1, l2, reason in _BASE_COLLISIONS:
|
||||
lines.append(f' <disable_collisions link1="{l1}" link2="{l2}" reason="{reason}"/>')
|
||||
|
||||
if tool_collisions:
|
||||
lines.append('')
|
||||
for l1, l2, reason in tool_collisions:
|
||||
lines.append(f' <disable_collisions link1="{l1}" link2="{l2}" reason="{reason}"/>')
|
||||
|
||||
lines += ['</robot>', '']
|
||||
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)
|
||||
Reference in New Issue
Block a user