diff --git a/src/iiwa_config/config/api_endpoints.yaml b/src/iiwa_config/config/api_endpoints.yaml index 71245ca..c78f075 100644 --- a/src/iiwa_config/config/api_endpoints.yaml +++ b/src/iiwa_config/config/api_endpoints.yaml @@ -42,7 +42,7 @@ endpoints: ros_name: cobot/move_to_named msg_type: iiwa_msgs/srv/MoveToNamedPose summary: "Переместить в именованную позу из SRDF" - description: "Перемещает робота в позу, определённую по имени в SRDF-файле (например, home, work)." + description: "Перемещает робота в позу, определённую по имени в SRDF-файле. Список доступных имён и значения суставов для каждой позиции возвращает GET /robot/positions." tags: [motion] timeout: 30.0 request_fields: diff --git a/src/iiwa_config/config/moveit/iiwa7.srdf b/src/iiwa_config/config/moveit/iiwa7.srdf index 3879ee9..ee69f82 100644 --- a/src/iiwa_config/config/moveit/iiwa7.srdf +++ b/src/iiwa_config/config/moveit/iiwa7.srdf @@ -45,6 +45,15 @@ + + + + + + + + + diff --git a/src/iiwa_config/config/moveit/named_positions_meta.yaml b/src/iiwa_config/config/moveit/named_positions_meta.yaml new file mode 100644 index 0000000..996160b --- /dev/null +++ b/src/iiwa_config/config/moveit/named_positions_meta.yaml @@ -0,0 +1,9 @@ +# Метаданные для именованных позиций из iiwa7.srdf +# Ключ совпадает с атрибутом name тега +named_positions: + home: + description: "Нулевое положение всех суставов - домашняя позиция робота" + work: + description: "Рабочее положение для выполнения задач" + transport: + description: "Позиция для транспортировки робота" \ No newline at end of file diff --git a/src/iiwa_web/iiwa_web/config_loader.py b/src/iiwa_web/iiwa_web/config_loader.py index a496cf3..a8e5af0 100644 --- a/src/iiwa_web/iiwa_web/config_loader.py +++ b/src/iiwa_web/iiwa_web/config_loader.py @@ -1,5 +1,6 @@ from __future__ import annotations +import xml.etree.ElementTree as ET import yaml from dataclasses import dataclass, field from pathlib import Path @@ -141,3 +142,50 @@ def load_api_config( )) return endpoints + + +@dataclass +class NamedPosition: + name: str + group: str + joints: dict[str, float] + description: str = "" + + +def load_named_positions( + srdf_path: str | None = None, + meta_path: str | None = None, + package: str = "iiwa_config", + srdf_relative: str = "config/moveit/iiwa7.srdf", + meta_relative: str = "config/moveit/named_positions_meta.yaml", +) -> list[NamedPosition]: + """Загружает именованные позиции из SRDF и объединяет с описаниями из YAML.""" + resolved_srdf = Path(srdf_path) if srdf_path else _resolve_path(package, srdf_relative) + resolved_meta = Path(meta_path) if meta_path else _resolve_path(package, meta_relative) + + tree = ET.parse(resolved_srdf) + root = tree.getroot() + + descriptions: dict[str, str] = {} + if resolved_meta.exists(): + with open(resolved_meta) as f: + meta = yaml.safe_load(f) or {} + for name, attrs in meta.get("named_positions", {}).items(): + descriptions[name] = attrs.get("description", "") + + positions: list[NamedPosition] = [] + for gs in root.findall("group_state"): + name = gs.get("name", "") + group = gs.get("group", "") + joints = { + j.get("name"): float(j.get("value", 0)) + for j in gs.findall("joint") + } + positions.append(NamedPosition( + name=name, + group=group, + joints=joints, + description=descriptions.get(name, ""), + )) + + return positions diff --git a/src/iiwa_web/iiwa_web/main.py b/src/iiwa_web/iiwa_web/main.py index 158dfa3..5949962 100644 --- a/src/iiwa_web/iiwa_web/main.py +++ b/src/iiwa_web/iiwa_web/main.py @@ -9,7 +9,7 @@ from sensor_msgs.msg import JointState from .dynamic_router import build_dynamic_router from .ros_node import CobotWebNode, get_bridge, set_bridge -from . import runner, trajectory +from . import runner, trajectory, positions def main(): @@ -23,10 +23,13 @@ def main(): endpoints_path = node.get_parameter('endpoints_path').value or None joint_limits_path = node.get_parameter('joint_limits_path').value or None + positions.init() + _schema_app = FastAPI() _schema_app.include_router(build_dynamic_router(endpoints_path, joint_limits_path)) _schema_app.include_router(runner.router) _schema_app.include_router(trajectory.router) + _schema_app.include_router(positions.router) mcp = FastMCP.from_fastapi(app=_schema_app) mcp_http = mcp.http_app(path='/mcp') @@ -41,6 +44,7 @@ def main(): app.include_router(build_dynamic_router(endpoints_path, joint_limits_path)) app.include_router(runner.router) app.include_router(trajectory.router) + app.include_router(positions.router) app.mount("/mcp", mcp_http) uvicorn.run(app, host=host, port=port) diff --git a/src/iiwa_web/iiwa_web/positions.py b/src/iiwa_web/iiwa_web/positions.py new file mode 100644 index 0000000..a728d14 --- /dev/null +++ b/src/iiwa_web/iiwa_web/positions.py @@ -0,0 +1,68 @@ +from fastapi import APIRouter +from pydantic import BaseModel, Field + +from .config_loader import load_named_positions + +router = APIRouter(tags=["robot"]) + + +class NamedPositionResponse(BaseModel): + name: str = Field( + description=( + "Идентификатор позиции. Передайте это значение в поле `name` запроса " + "`POST /robot/move/named`, чтобы переместить робота в эту позу." + ) + ) + group: str = Field( + description="Группа планирования MoveIt, к которой относится позиция (обычно `iiwa_arm`)." + ) + joints: dict[str, float] = Field( + description=( + "Целевые углы суставов в радианах. Ключи: joint1..joint7. " + "Используйте эти значения для оценки конфигурации перед движением " + "или как основу для `POST /robot/move/joints`." + ) + ) + description: str = Field( + description="Человекочитаемое описание назначения позиции." + ) + + +_srdf_path: str | None = None +_meta_path: str | None = None + + +def init(srdf_path: str | None = None, meta_path: str | None = None) -> None: + global _srdf_path, _meta_path + _srdf_path = srdf_path + _meta_path = meta_path + + +@router.get( + "/robot/positions", + response_model=list[NamedPositionResponse], + summary="Список заготовленных именованных позиций робота", + description=( + "Возвращает все именованные позиции (`group_state`) из SRDF-файла конфигурации робота. " + "\n\n" + "**Типичный рабочий процесс для агента:**\n" + "1. Вызовите этот эндпоинт, чтобы узнать доступные позиции и их суставные значения.\n" + "2. Выберите подходящую позицию по полю `description` и значениям `joints`.\n" + "3. Передайте поле `name` выбранной позиции в `POST /robot/move/named`, " + "чтобы переместить робота туда.\n" + "\n" + "Позиции определены статически в SRDF и гарантированно безопасны с точки зрения " + "столкновений и кинематики." + ), +) +def get_named_positions() -> list[NamedPositionResponse]: + positions = load_named_positions(srdf_path=_srdf_path, meta_path=_meta_path) + return [ + NamedPositionResponse( + name=p.name, + group=p.group, + joints=p.joints, + description=p.description, + ) + for p in positions + ]