From 86e6e801e9249ab500bdc8ecbec7a12fb00eb3bd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Wed, 20 May 2026 14:43:20 +1000 Subject: [PATCH] Add robot setup command and configuration management for cobot-setting.yaml --- cobot-setting.yaml | 73 +++++++ cobot/cli.py | 50 ++++- cobot/commands/robot_setup.py | 290 +++++++++++++++++++++++++++ cobot/commands/setup.py | 2 + cobot/tui.py | 15 +- docker/jazzy/ros-iiwa7-webots/run.sh | 8 +- setup.py | 3 +- 7 files changed, 435 insertions(+), 6 deletions(-) create mode 100644 cobot-setting.yaml create mode 100644 cobot/commands/robot_setup.py diff --git a/cobot-setting.yaml b/cobot-setting.yaml new file mode 100644 index 0000000..92c57ac --- /dev/null +++ b/cobot-setting.yaml @@ -0,0 +1,73 @@ +robot: + name: "iiwa7" + ip: "192.170.10.10" + port: 30200 + command_mode: "position" # torque, position + fri_cycle_ms: 10 # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц) + joint_position_tau: 0.04 # EMA фильтр позиций [с]: сглаживает команды перед отправкой в FRI + joint_velocity_tau: 0.01 # EMA фильтр скорости [с]: убирает выбросы конечных разностей + active_controller: "jtc" # "jtc" = MoveIt/JointTrajectoryController, "forward" = ForwardCommandController + description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro + + +digital_twin: + webots: + world: pkg://iiwa_description/worlds/iiwa.wbt + # world: pkg://iiwa_description/worlds/simple_world.wbt + transform: "-0.25 0 0.79" + rotation: "0 0 1 0" + controller_timer: "50" + + cameras: + - pkg://iiwa_config/config/cameras/d455_top.yaml + + rviz: + config: pkg://iiwa_config/config/rviz/rviz_moveit.rviz + +controller: + controller_path: pkg://iiwa_config/config/moveit/iiwa_controller.yaml + + moveit: + srdf: pkg://iiwa_config/config/moveit/iiwa7.srdf + kinematics: pkg://iiwa_config/config/moveit/kinematics.yaml + joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml + pilz_limits: pkg://iiwa_config/config/moveit/pilz_cartesian_limits.yaml + initial_positions: pkg://iiwa_config/config/moveit/initial_positions.yaml + moveit_controllers: pkg://iiwa_config/config/moveit/moveit_controllers.yaml + moveit_cpp: pkg://iiwa_config/config/moveit/moveit_cpp.yaml + + +planning: + pose_link: "tcp" # TCP-линк для декартовых целей + planning_group: "iiwa_arm" # Группа планирования из SRDF + default_frame: "base_link" # Система отсчёта по умолчанию + default_planner: "ompl" # Планировщик по умолчанию + planning_attempts: 3 # Число попыток планирования + +foxglove: + enabled: true # Запускать ли foxglove_bridge вместе с роботом + port: 8765 # WebSocket-порт, к которому подключается Foxglove Studio (по умолчанию 8765) + debug: false # Включить подробное логирование bridge-процесса + address: 0.0.0.0 # Адрес, на котором слушает сервер. 0.0.0.0 — все интерфейсы, 127.0.0.1 — только локально + tls: false # Включить TLS-шифрование соединения (нужны certfile + keyfile) + certfile: "" # Путь к SSL-сертификату (нужен только при tls: true)Путь к SSL-сертификату (нужен только при tls: true) + keyfile: "" # Путь к SSL-сертификату (нужен только при tls: true) + topic_whitelist: ['.*'] # Regex-список топиков, которые bridge публикует клиенту + param_whitelist: ['.*'] # Regex-список ROS-параметров, видимых клиенту + service_whitelist: ['.*'] # Regex-список сервисов, доступных клиенту + client_topic_whitelist: ['.*'] # Regex-список топиков, в которые клиент может публиковать (clientPublish) + min_qos_depth: 1 # Минимальная глубина QoS-очереди при подписке bridge на топик + max_qos_depth: 10 # Максимальная глубина QoS-очереди + num_threads: 0 # Число потоков исполнения. 0 = автоматически по числу CPU + send_buffer_limit: 10000000 # Максимальный размер буфера отправки в байтах (защита от OOM при медленном клиенте) + use_sim_time: false # Использовать симуляционное время /clock вместо системного + capabilities: # Список возможностей, открытых клиенту + - 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 # Не падать, если нода не отвечает на запросы параметров (защита от зависания при старте) diff --git a/cobot/cli.py b/cobot/cli.py index 165cab9..7660993 100644 --- a/cobot/cli.py +++ b/cobot/cli.py @@ -1,17 +1,62 @@ import argparse -import importlib import sys from cobot.commands import docker_setup as cmd_docker_setup from cobot.commands import doc_setup as cmd_doc_setup +from cobot.commands import robot_setup as cmd_robot_setup from cobot.commands import setup as cmd_setup +# Command groups shown in --help output. +# Add new commands here when introducing other categories. +_GROUPS = [ + ("Setup", [ + ("setup", "run doc-setup + docker-setup + robot-setup in one go"), + ("docker-setup", "build or pull Docker images for KUKA iiwa7"), + ("doc-setup", "deploy or stop the MkDocs documentation server"), + ("robot-setup", "configure cobot-setting.yaml interactively"), + ]), +] + +_DESCRIPTION = "Lightweight Cobot" + + +class _GroupedHelpAction(argparse.Action): + def __init__(self, option_strings, dest, default=None, required=False, help=None): + super().__init__( + option_strings=option_strings, + dest=dest, + nargs=0, + default=default, + required=required, + help=help, + ) + + def __call__(self, parser, namespace, values, option_string=None): + print(f"usage: cobot [-h] ...\n") + print(f"{_DESCRIPTION}\n") + for group_title, commands in _GROUPS: + print(f"{group_title} commands:") + for cmd, help_text in commands: + print(f" {cmd:<22} {help_text}") + print() + print("options:") + print(" -h, --help show this help message and exit") + parser.exit() + def main(): parser = argparse.ArgumentParser( prog="cobot", - description="Lightweight Cobot - Cobot ROS2 Control Framework", + description=_DESCRIPTION, + add_help=False, ) + parser.add_argument( + "-h", "--help", + action=_GroupedHelpAction, + default=argparse.SUPPRESS, + help="show this help message and exit", + ) + subparsers = parser.add_subparsers(dest="command", metavar="") subparsers.required = True @@ -25,3 +70,4 @@ def _register_commands(subparsers): cmd_setup.register(subparsers) cmd_docker_setup.register(subparsers) cmd_doc_setup.register(subparsers) + cmd_robot_setup.register(subparsers) diff --git a/cobot/commands/robot_setup.py b/cobot/commands/robot_setup.py new file mode 100644 index 0000000..7ed4c6b --- /dev/null +++ b/cobot/commands/robot_setup.py @@ -0,0 +1,290 @@ +from __future__ import annotations + +import argparse +import sys +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any, Callable, List, Optional, Tuple + +from rich.console import Console +from ruamel.yaml import YAML +from textual.app import App + +from cobot.tui import SCREEN_CSS, InputScreen, PickScreen + +_console = Console() +_PROJECT_DIR = Path(__file__).parent.parent.parent +_CONFIG_PATH = _PROJECT_DIR / "cobot-setting.yaml" + +_yaml = YAML() +_yaml.preserve_quotes = True + + +# --------------------------------------------------------------------------- +# Field descriptors +# --------------------------------------------------------------------------- + +@dataclass +class _Field: + key: str # dot-separated path within the block, e.g. "webots.world" + question: str + default: Any + note: str = "" + options: Optional[List[str]] = None # if set → PickScreen, else → InputScreen + + def label(self) -> str: + return self.key.split(".")[-1] + + +@dataclass +class _Block: + yaml_key: str # top-level key in cobot-setting.yaml + title: str # shown in "Configure ?" prompt + fields: List[_Field] + + +# --------------------------------------------------------------------------- +# Block definitions — bottom to top order +# --------------------------------------------------------------------------- + +_BLOCKS: List[_Block] = [ + _Block( + yaml_key="foxglove", + title="Foxglove bridge", + fields=[ + _Field("enabled", "Enable Foxglove bridge?", "true", + note="Start foxglove_bridge alongside the robot node", + options=["true", "false"]), + _Field("port", "WebSocket port:", "8765", + note="Port Foxglove Studio connects to (default 8765)"), + _Field("address", "Listen address:", "0.0.0.0", + note="0.0.0.0 = all interfaces, 127.0.0.1 = localhost only", + options=["0.0.0.0", "127.0.0.1"]), + _Field("use_sim_time", "Use simulation time (/clock)?", "false", + note="Subscribe to /clock instead of using wall time", + options=["false", "true"]), + _Field("debug", "Enable verbose bridge logging?", "false", + options=["false", "true"]), + _Field("num_threads", "Executor threads (0 = auto):", "0"), + ], + ), + _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"), + _Field("default_planner", "Default planner:", "ompl", + options=["ompl", "pilz_industrial_motion_planner", "chomp"]), + _Field("planning_attempts", "Planning attempts:", "3"), + ], + ), + _Block( + yaml_key="digital_twin", + title="Digital twin (Webots / RViz)", + fields=[ + _Field("webots.transform", "Robot transform in Webots scene (x y z, metres):", "-0.25 0 0.79"), + _Field("webots.rotation", "Robot rotation in Webots scene (ax ay az angle):", "0 0 1 0"), + _Field("webots.controller_timer", "Webots controller step timer (ms):", "50"), + ], + ), + _Block( + yaml_key="robot", + title="Robot connection", + fields=[ + _Field("name", "Robot model name:", "iiwa7"), + _Field("ip", "Robot IP address:", "192.170.10.2", + note="IP of the KUKA controller on the FRI network interface"), + _Field("port", "FRI port:", "30200"), + _Field("command_mode", "Command mode:", "position", + note="position = joint position control, torque = joint torque control", + options=["position", "torque"]), + _Field("fri_cycle_ms", "FRI cycle time (ms):", "10", + note="5 ms = 200 Hz, 10 ms = 100 Hz", + options=["10", "5"]), + _Field("active_controller", "Active ROS controller:", "jtc", + note="jtc = JointTrajectoryController (MoveIt), forward = ForwardCommandController", + options=["jtc", "forward"]), + _Field("joint_position_tau", "Position EMA filter τ (s):", "0.04", + note="Smooths position commands before sending to FRI"), + _Field("joint_velocity_tau", "Velocity EMA filter τ (s):", "0.01", + note="Removes spikes from finite-difference velocity estimation"), + ], + ), +] + + +# --------------------------------------------------------------------------- +# Wizard app +# --------------------------------------------------------------------------- + +def _coerce(value: str, original: Any) -> Any: + """Try to preserve the original YAML scalar type.""" + if isinstance(original, bool): + return value.lower() == "true" + if isinstance(original, int): + try: + return int(value) + except ValueError: + return value + if isinstance(original, float): + try: + return float(value) + except ValueError: + return value + return value + + +def _get_nested(mapping: Any, path: str) -> Any: + keys = path.split(".") + cur = mapping + for k in keys: + if cur is None or k not in cur: + return None + cur = cur[k] + return cur + + +def _set_nested(mapping: Any, path: str, value: Any) -> None: + keys = path.split(".") + cur = mapping + for k in keys[:-1]: + cur = cur[k] + original = cur[keys[-1]] + cur[keys[-1]] = _coerce(value, original) + + +class _Wizard(App[bool]): + CSS = SCREEN_CSS + + def __init__(self, data: Any): + super().__init__() + self._data = data + self._blocks = list(_BLOCKS) # copy so we can pop + self._block_idx = 0 + self._field_idx = 0 + self._current_block: Optional[_Block] = None + self._pending_fields: List[_Field] = [] + + def on_mount(self) -> None: + self._next_block() + + # ------------------------------------------------------------------ + # Block-level flow + # ------------------------------------------------------------------ + + def _next_block(self) -> None: + if self._block_idx >= len(self._blocks): + self.exit(True) + return + block = self._blocks[self._block_idx] + total = len(self._blocks) + step = f"Block {self._block_idx + 1} of {total}" + self.push_screen( + PickScreen( + step, + f"Configure {block.title}?", + ["Yes", "No"], + "Yes", + ), + lambda v: self._got_block_choice(v, block), + ) + + def _got_block_choice(self, v: Optional[str], block: _Block) -> None: + if v is None: + self.exit(False) + return + self._block_idx += 1 + if v == "Yes": + self._current_block = block + self._pending_fields = list(block.fields) + self._field_idx = 0 + self._next_field() + else: + self._next_block() + + # ------------------------------------------------------------------ + # Field-level flow + # ------------------------------------------------------------------ + + def _next_field(self) -> None: + if not self._pending_fields: + self._next_block() + return + + f = self._pending_fields[0] + block = self._current_block + total_blocks = len(self._blocks) + block_num = self._block_idx # already incremented + self._field_idx += 1 + field_num = self._field_idx + total_fields = len(block.fields) + + step = f"Block {block_num} of {total_blocks} · Field {field_num} of {total_fields}" + + # Resolve current value from loaded YAML as the pre-filled default + yaml_val = _get_nested(self._data[block.yaml_key], f.key) + current = str(yaml_val) if yaml_val is not None else f.default + + if f.options: + # Make the current value the default selection + default_opt = current if current in f.options else f.options[0] + screen = PickScreen(step, f.question, f.options, default_opt, note=f.note) + else: + screen = InputScreen(step, f.question, current, note=f.note) + + self.push_screen(screen, lambda v, _f=f: self._got_field(v, _f)) + + def _got_field(self, v: Optional[str], f: _Field) -> None: + if v is None: + self.exit(False) + return + block = self._current_block + _set_nested(self._data[block.yaml_key], f.key, v) + self._pending_fields.pop(0) + self._next_field() + + +# --------------------------------------------------------------------------- +# YAML read / write +# --------------------------------------------------------------------------- + +def _load_config() -> Any: + with open(_CONFIG_PATH, "r", encoding="utf-8") as fh: + return _yaml.load(fh) + + +def _save_config(data: Any) -> None: + with open(_CONFIG_PATH, "w", encoding="utf-8") as fh: + _yaml.dump(data, fh) + + +# --------------------------------------------------------------------------- +# CLI entry points +# --------------------------------------------------------------------------- + +def register(subparsers: argparse._SubParsersAction) -> None: + p = subparsers.add_parser("robot-setup", help="Configure cobot-setting.yaml interactively") + p.set_defaults(func=run) + + +def run(args: argparse.Namespace) -> None: + if not _CONFIG_PATH.exists(): + _console.print(f"[red]Config not found:[/red] {_CONFIG_PATH}") + sys.exit(1) + + data = _load_config() + ok = _Wizard(data).run() + if not ok: + _console.print("[yellow]Setup cancelled.[/yellow]") + return + + _save_config(data) + _console.print(f"\n[green]Configuration saved:[/green] {_CONFIG_PATH}") + _console.print( + " Start the robot container with: [bold]cobot robot-setup[/bold] " + "then run [bold]./docker/jazzy/ros-iiwa7-webots/run.sh[/bold]" + ) diff --git a/cobot/commands/setup.py b/cobot/commands/setup.py index 65b8010..a9e0f8d 100644 --- a/cobot/commands/setup.py +++ b/cobot/commands/setup.py @@ -2,6 +2,7 @@ import argparse from cobot.commands.doc_setup import run as _doc_setup from cobot.commands.docker_setup import run as _docker_setup +from cobot.commands.robot_setup import run as _robot_setup def register(subparsers): @@ -12,3 +13,4 @@ def register(subparsers): def run(args: argparse.Namespace) -> None: _doc_setup(args) _docker_setup(args) + _robot_setup(args) diff --git a/cobot/tui.py b/cobot/tui.py index 87c77f5..865079d 100644 --- a/cobot/tui.py +++ b/cobot/tui.py @@ -22,6 +22,11 @@ Screen { margin-top: 1; margin-bottom: 1; } +#note { + color: $text-muted; + text-style: dim; + margin-bottom: 1; +} RadioSet { height: auto; border: none; @@ -40,16 +45,19 @@ class PickScreen(Screen[Optional[str]]): Binding("escape", "abort", "Cancel"), ] - def __init__(self, step: str, question: str, options: List[str], default: str): + def __init__(self, step: str, question: str, options: List[str], default: str, note: str = ""): super().__init__() self._step = step self._question = question self._options = options self._default = default + self._note = note def compose(self) -> ComposeResult: yield Static(self._step, id="step") yield Static(self._question, id="question") + if self._note: + yield Static(self._note, id="note") with RadioSet(id="choices"): for opt in self._options: yield RadioButton(opt, value=(opt == self._default)) @@ -72,15 +80,18 @@ class InputScreen(Screen[Optional[str]]): Binding("escape", "abort", "Cancel"), ] - def __init__(self, step: str, question: str, default: str): + def __init__(self, step: str, question: str, default: str, note: str = ""): super().__init__() self._step = step self._question = question self._default = default + self._note = note def compose(self) -> ComposeResult: yield Static(self._step, id="step") yield Static(self._question, id="question") + if self._note: + yield Static(self._note, id="note") yield Input(id="value", value=self._default) yield Footer() diff --git a/docker/jazzy/ros-iiwa7-webots/run.sh b/docker/jazzy/ros-iiwa7-webots/run.sh index ff327c8..532117a 100755 --- a/docker/jazzy/ros-iiwa7-webots/run.sh +++ b/docker/jazzy/ros-iiwa7-webots/run.sh @@ -12,9 +12,14 @@ set -e -IMAGE="${WEBOTS_IMAGE:-evilfisru/lwc:webots-jazzy}" +IMAGE="${WEBOTS_IMAGE:-evilfisru/lwc:webots-jazzy-dev}" GPU_MODE="software" +# Resolve the project root (two levels above this script: docker/jazzy/ros-iiwa7-webots → project root) +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PROJECT_ROOT="$(cd "${SCRIPT_DIR}/../../.." && pwd)" +CONFIG_FILE="${PROJECT_ROOT}/cobot-setting.yaml" + # Parse --gpu flag if [[ "${1}" == "--gpu" ]]; then GPU_MODE="${2:?--gpu requires an argument: nvidia|mesa|software}" @@ -58,5 +63,6 @@ docker run -it --rm \ -e QT_X11_NO_MITSHM=1 \ "${RENDER_FLAGS[@]}" \ -v /tmp/.X11-unix:/tmp/.X11-unix:rw \ + ${CONFIG_FILE:+-v "${CONFIG_FILE}:/ros2_ws/cobot-setting.yaml:ro"} \ "${IMAGE}" \ "$@" diff --git a/setup.py b/setup.py index 92a0a5c..f423118 100644 --- a/setup.py +++ b/setup.py @@ -3,11 +3,12 @@ from setuptools import setup, find_packages setup( name="lightweight-cobot", version="0.1.0", - description="Lightweight Cobot — KUKA iiwa7 ROS2 Control Framework", + description="Lightweight Cobot", packages=find_packages(), python_requires=">=3.11", install_requires=[ "textual", + "ruamel.yaml", ], entry_points={ "console_scripts": [