From f7dff2e4840f40afa65e01ebf1837b720a29275c 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: Mon, 14 Sep 2026 14:36:46 +1000 Subject: [PATCH] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=D0=BE=20=D1=83=D0=BF=D1=80=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD?= =?UTF-8?q?=D0=B8=D0=B5=20=D1=80=D0=BE=D0=B1=D0=BE=D1=82=D0=BE=D0=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 5 +- README.md | 49 +- configs/scene.yaml | 2 +- src/iiwa_genesis/cli.py | 125 ++++- src/iiwa_genesis/control.py | 286 ++++++++++++ src/iiwa_genesis/control_gui.py | 183 ++++++++ src/iiwa_genesis/scene.py | 153 +++++- tests/test_control.py | 800 ++++++++++++++++++++++++++++++++ 8 files changed, 1589 insertions(+), 14 deletions(-) create mode 100644 src/iiwa_genesis/control.py create mode 100644 src/iiwa_genesis/control_gui.py create mode 100644 tests/test_control.py diff --git a/.gitignore b/.gitignore index effb4b6..5171428 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,7 @@ outputs/ .cache/ .codex/ .agents/ -iiwa_description \ No newline at end of file +iiwa_description/ +docs/ + +AGENTS.md \ No newline at end of file diff --git a/README.md b/README.md index 834ee3f..ef2a13a 100644 --- a/README.md +++ b/README.md @@ -17,7 +17,7 @@ uv run python run_scene.py --config configs/scene.yaml ```bash uv sync --extra cpu --locked -uv run --extra cpu python run_scene.py --headless --steps 4 --save-frames +uv run --extra cpu python run_scene.py --backend cpu --headless --steps 4 --save-frames uv run --extra cpu pytest ``` @@ -35,6 +35,46 @@ uv run pytest IIWA_GENESIS_SMOKE=1 uv run pytest tests/test_genesis_smoke.py ``` +Для управления во время viewer-запуска добавьте `--interactive`, а для +отдельного окна управления — `--control-window` (их можно использовать вместе): + +```bash +uv run python run_scene.py --viewer --control-window +``` + +Окно содержит выбор режима, семь полей суставов A1..A7, шесть Cartesian-полей +`x y z roll pitch yaw`, ползунок скорости 0–100%, а также Send, Stop и Close. +Скорость ограничивает каждый сустав по его velocity limit из выбранного URDF; +100% использует полный limit, а 0% и кнопка Stop отменяют движение и удерживают +текущую целевую позу. Ошибка запуска окна не останавливает сцену. + +При `--interactive` в терминале выберите режим и отправляйте команды; terminal +команды используют скорость 100%, а `set` использует выбранный режим: + +```text +mode joints +set 0 0.2 0 1.0 0 0 0 +mode cartesian +set 0.35 0 0.55 0 1.57 0 +``` + +В режиме joints значения — A1..A7 в радианах; в режиме cartesian — +`x y z roll pitch yaw`. После выбора режима можно вводить и только числа, без `set`. + +Cartesian-поза задаётся для TCP в системе координат базы робота (метры и +радианы), а не в мировой системе. Также доступны однострочные формы +`joints ...`, `cartesian ...`, `help` и `quit`. Для Cartesian-команд Genesis +решает IK и плавно ведёт суставы к целевой позе через position controller. +Для сцены без захвата используется `link7`, с захватом — `tcp`. +Команды вне суставных limits и решения IK с ошибкой положения больше 0,5 мм +или ориентации больше 0,005 рад отклоняются; предыдущее движение продолжается. +Stop удерживает последнюю отправленную контроллеру суставную цель. +За один шаг обрабатывается до 16 сообщений из каждой очереди ввода, чтобы +поток команд не блокировал физику. В прочитанной порции `quit` имеет приоритет +и завершает цикл до обработки GUI и следующего шага. Stop отменяет ожидающие +движения терминала и предшествующие ему GUI-команды в этой порции; GUI-команды +после Stop выполняются не раньше следующего шага. + Headless по умолчанию выполняет 120 шагов, viewer работает до закрытия окна, Ctrl+C или Q/Esc в окне кадров. `output.preview` включает окна RGB и глубины. Превью работает в отдельном процессе: оконные циклы OpenCV и Genesis не мешают @@ -43,7 +83,8 @@ Ctrl+C или Q/Esc в окне кадров. `output.preview` включает 30 FPS — частота по времени симуляции; реальная скорость зависит от оборудования. Headless всё равно требует рабочий OpenGL/EGL для RGB. Используется EGL, если `PYOPENGL_PLATFORM` не задан пользователем. GPU требует совместимого драйвера; -`backend: cpu` выбран для переносимого первого запуска. Системный драйвер и +в текущем `configs/scene.yaml` выбран `backend: gpu`. Для запуска на CPU +передайте `--backend cpu` или выберите `scene.backend: cpu`. Системный драйвер и библиотеки OpenGL предоставляются ОС, их uv не устанавливает. Обычные тесты проверяют URDF, конфигурацию и математику; opt-in smoke-тесты реально строят оба мира и сохраняют RGB-D на CPU/EGL. Первичная компиляция @@ -66,8 +107,8 @@ Python overlays, а pytest не загружает внешние плагины сенсора дополнительно смещена на `[0, 0.014, 0.027]` м: передняя плоскость находится за корпусом толщиной 26 мм. Это геометрическое приближение, исключающее измерение собственного корпуса, а не заводская калибровка. -Нулевая гравитация удерживает выбранную позу без контроллера: v1 предназначена -для осмотра сцены и получения изображений, а не динамического управления. +Сцена использует нулевую гравитацию и суставной position controller для +удержания позы и выполнения команд с ограниченной скоростью. При нулевых углах суставов фланец и камера инструмента направлены вверх. Для осмотра стола этой камерой задайте соответствующую рабочую позу в `robot.joints`. diff --git a/configs/scene.yaml b/configs/scene.yaml index 8f715d2..a73f703 100644 --- a/configs/scene.yaml +++ b/configs/scene.yaml @@ -1,7 +1,7 @@ scene: # mode: without_gripper # with_gripper automatically adds the tool camera mode: with_gripper # without_gripper automatically adds the tool camera - backend: cpu # gpu on a machine with a supported GPU + backend: gpu # gpu on a machine with a supported GPU viewer: true dt: 0.008333333333333333 # 120 Hz simulation clock seed: 42 diff --git a/src/iiwa_genesis/cli.py b/src/iiwa_genesis/cli.py index 589aaaf..4ee3e05 100644 --- a/src/iiwa_genesis/cli.py +++ b/src/iiwa_genesis/cli.py @@ -2,10 +2,15 @@ import argparse from datetime import datetime, timezone import os from pathlib import Path +from queue import Empty import time from yaml import YAMLError from .config import ROOT, load_config +from .control import ModeCommand, QuitCommand, TerminalInput, help_text, parse_command + + +MAX_INPUTS_PER_TICK = 16 def main(): @@ -18,6 +23,10 @@ def main(): parser.add_argument("--save-frames", action="store_true") parser.add_argument("--steps", type=int, help="finite simulation steps (headless default: 120)") parser.add_argument("--backend", choices=("cpu", "gpu")) + parser.add_argument("--interactive", action="store_true", + help="accept joint/TCP commands from the terminal while running") + parser.add_argument("--control-window", action="store_true", + help="open a separate GUI window for joint/TCP commands") args = parser.parse_args() try: cfg = load_config(args.config) @@ -35,11 +44,33 @@ def main(): from .scene import build_scene gs.init(backend=getattr(gs, args.backend or cfg["scene"]["backend"]), seed=cfg["scene"]["seed"], logging_level="warning") - from .preview import Preview - preview = Preview() if viewer and cfg["output"]["preview"] else None + preview = None + control_window = None try: + from .preview import Preview + preview = Preview() if viewer and cfg["output"]["preview"] else None print("Building scene; first launch compiles Genesis kernels...", flush=True) scene, robot, cameras = build_scene(gs, cfg, viewer) + dof_indices = [robot.get_joint(f"joint{i}").dofs_idx_local[0] for i in range(1, 8)] + from .scene import SceneMotionController + controller = SceneMotionController(robot, dof_indices, cfg) + terminal = TerminalInput() if args.interactive else None + if terminal: + terminal.start() + print(help_text(), flush=True) + control_window_error = False + gui_commands_enabled = False + gui_diagnostics_enabled = False + if args.control_window: + try: + from .control_gui import ControlGui + control_window = ControlGui() + control_window.start() + gui_commands_enabled = True + gui_diagnostics_enabled = True + except (ImportError, OSError, RuntimeError) as exc: + control_window_error = True + print(f"Control window error: {exc}", flush=True) directory = cfg["output"]["directory"] / datetime.now(timezone.utc).strftime("%Y%m%dT%H%M%S_%fZ") print(f"Mode: {cfg['scene']['mode']}; cameras: {', '.join(c.name for c in cameras)}; Genesis {gs.__version__}; backend {gs.backend}") if save: @@ -49,9 +80,87 @@ def main(): dt = cfg["scene"]["dt"] start = time.monotonic() step = 0 + pending_gui = [] while steps is None or step < steps: + terminal_lines = [] + quit_requested = False + if terminal: + for _ in range(MAX_INPUTS_PER_TICK): + line = terminal.poll() + if line is None: + break + terminal_lines.append(line) + # Recognize quit before executing any pending movement/IK. + try: + if isinstance(parse_command(line, controller.mode), QuitCommand): + quit_requested = True + break + except (TypeError, ValueError): + pass # Parse/report in order, after preceding mode changes. + if quit_requested: + break + gui_commands = pending_gui + pending_gui = [] + if control_window: + for _ in range(MAX_INPUTS_PER_TICK - len(gui_commands)): + if not gui_commands_enabled: + break + try: + gui_commands.append(control_window.commands.get_nowait()) + except Empty: + break + except (AttributeError, EOFError, OSError, RuntimeError, ValueError) as exc: + gui_commands_enabled = False + print(f"Control window command queue error: {exc}", flush=True) + break + stop_index = next((index for index in range(len(gui_commands) - 1, -1, -1) + if isinstance(gui_commands[index], dict) + and gui_commands[index].get("type") == "stop"), None) + if stop_index is not None: + # Stop wins this tick; later GUI commands can resume next tick. + pending_gui = gui_commands[stop_index + 1:] + controller.apply_gui(gui_commands[stop_index]) + gui_commands = [] + for line in terminal_lines: + try: + command = parse_command(line, controller.mode) + if command is None: + continue + if getattr(command, "mode", None) == "help": + print(help_text(), flush=True) + elif isinstance(command, ModeCommand) or stop_index is None: + controller.apply(command) + if isinstance(command, ModeCommand): + print(f"Control mode: {command.mode}", flush=True) + except (TypeError, ValueError, RuntimeError) as exc: + print(f"Command error: {exc}", flush=True) + for command in gui_commands: + try: + controller.apply_gui(command) + except (AttributeError, KeyError, RuntimeError, TypeError, ValueError) as exc: + print(f"Command error (control window): {exc}", flush=True) + if control_window: + for _ in range(MAX_INPUTS_PER_TICK): + if not gui_diagnostics_enabled: + break + try: + diagnostic = control_window.diagnostics.get_nowait() + except Empty: + break + except (AttributeError, EOFError, OSError, RuntimeError, ValueError) as exc: + gui_diagnostics_enabled = False + print(f"Control window diagnostic queue error: {exc}", flush=True) + break + if not control_window_error: + control_window_error = True + message = diagnostic.get("message", "startup failed") if isinstance(diagnostic, dict) else diagnostic + print(f"Control window error: {message}", flush=True) + if control_window.status == "failed" and not control_window_error: + control_window_error = True + print("Control window error: startup failed", flush=True) if viewer and not scene.viewer.is_alive(): break + controller.step() scene.step() step += 1 timestamp = step * dt @@ -73,9 +182,15 @@ def main(): except KeyboardInterrupt: pass finally: - if preview: - preview.close() - gs.destroy() + try: + for name, resource in (("Control window", control_window), ("Preview", preview)): + if resource is not None: + try: + resource.close() + except Exception as exc: + print(f"{name} close error: {exc}", flush=True) + finally: + gs.destroy() if __name__ == "__main__": diff --git a/src/iiwa_genesis/control.py b/src/iiwa_genesis/control.py new file mode 100644 index 0000000..526fd3a --- /dev/null +++ b/src/iiwa_genesis/control.py @@ -0,0 +1,286 @@ +"""Interactive terminal control for the seven-axis iiwa.""" +from dataclasses import dataclass +import numbers +import shlex +import threading +from queue import Empty, Queue +import sys +import math + +import numpy as np +from scipy.spatial.transform import Rotation + + +@dataclass(frozen=True) +class ModeCommand: + mode: str + + +@dataclass(frozen=True) +class JointCommand: + angles: tuple[float, ...] + + +@dataclass(frozen=True) +class CartesianCommand: + position: tuple[float, float, float] + rpy: tuple[float, float, float] + + +@dataclass(frozen=True) +class QuitCommand: + pass + + +class GuiCommandQueue: + """Adapt a multiprocessing queue to plain GUI command dictionaries.""" + + def __init__(self, queue=None): + if queue is None: + import multiprocessing as mp + queue = mp.get_context("spawn").Queue() + self.queue = queue + + def send_joint(self, angles, speed_percent): + self._send_values("joints", angles, speed_percent, 7) + + def send_cartesian(self, values, speed_percent): + self._send_values("cartesian", values, speed_percent, 6) + + def _send_values(self, command_type, values, speed_percent, count): + if len(values) != count: + raise ValueError(f"{command_type} expects {count} values") + try: + values = [float(value) for value in values] + speed_percent = float(speed_percent) + except (TypeError, ValueError) as exc: + raise ValueError("GUI values must be numeric") from exc + if not all(math.isfinite(value) for value in values + [speed_percent]): + raise ValueError("GUI values must be finite") + if not 0 <= speed_percent <= 100: + raise ValueError("speed_percent must be between 0 and 100") + self.queue.put({"type": "stop"} if speed_percent == 0 else { + "type": command_type, "values": values, + "speed_percent": speed_percent, + }) + + def send_stop(self): + self.queue.put({"type": "stop"}) + + def send_close(self): + self.queue.put({"type": "close"}) + + def poll(self): + try: + return self.queue.get(timeout=0.05) + except (Empty, OSError, ValueError): + return None + + def close(self): + close = getattr(self.queue, "close", None) + if close is not None: + close() + + +def _scalar_float(value, name): + if isinstance(value, (bool, np.bool_)) or not isinstance(value, numbers.Real): + raise ValueError(f"{name} must be a real numeric scalar") + try: + value = float(value) + except (TypeError, ValueError, OverflowError) as exc: + raise ValueError(f"{name} must be numeric") from exc + if not np.isfinite(value): + raise ValueError(f"{name} must be finite") + return value + + +class VelocityLimitedMotion: + """Generate joint targets that respect scaled URDF velocity limits.""" + + def __init__(self, limits): + self.limits = np.asarray(limits, dtype=float) + if self.limits.ndim != 1 or not self.limits.size: + raise ValueError("limits must be a non-empty sequence") + if not np.isfinite(self.limits).all() or (self.limits <= 0).any(): + raise ValueError("limits must be positive and finite") + self.target = None + self._speed_limits = None + self._stopped = False + self.complete = True + + def set_target(self, q, speed_percent): + speed_percent = _scalar_float(speed_percent, "speed_percent") + if not 0 < speed_percent <= 100: + raise ValueError("speed_percent must be between 0 and 100") + target = np.asarray(q, dtype=float) + if target.shape != self.limits.shape or not np.isfinite(target).all(): + raise ValueError("target must match limits and contain finite values") + self.target = target.copy() + self._speed_limits = self.limits * (speed_percent / 100) + self._stopped = False + self.complete = False + + def step(self, current_q, dt): + if self.target is None: + raise RuntimeError("set_target must be called before step") + dt = _scalar_float(dt, "dt") + if dt < 0: + raise ValueError("dt must be non-negative") + current = np.asarray(current_q, dtype=float) + if current.shape != self.limits.shape or not np.isfinite(current).all(): + raise ValueError("current_q must match limits and contain finite values") + if self._stopped: + return self.target.copy(), True + delta = self.target - current + progress = np.minimum(np.abs(delta), self._speed_limits * dt) + position = current + np.sign(delta) * progress + self.complete = bool(np.array_equal(position, self.target)) + return position, self.complete + + def stop(self, current_q): + current = np.asarray(current_q, dtype=float) + if current.shape != self.limits.shape or not np.isfinite(current).all(): + raise ValueError("current_q must match limits and contain finite values") + self.target = current.copy() + self._speed_limits = np.zeros_like(self.limits) + self._stopped = True + self.complete = True + + +def _numbers(values, count, name): + if len(values) != count: + raise ValueError(f"{name} expects {count} values") + try: + result = tuple(float(value) for value in values) + except ValueError as exc: + raise ValueError(f"{name} values must be numbers") from exc + if not np.isfinite(result).all(): + raise ValueError(f"{name} values must be finite") + return result + + +def parse_command(line, mode=None): + """Parse one terminal command; Cartesian values are TCP pose in base frame.""" + try: + words = shlex.split(line) + except ValueError as exc: + raise ValueError(f"invalid command quoting: {exc}") from exc + if not words: + return None + command = words[0].lower() + arguments = words[1:] + if command == "mode": + if len(words) != 2 or words[1].lower() not in ("joints", "cartesian"): + raise ValueError("mode must be joints or cartesian") + return ModeCommand(words[1].lower()) + if command in ("quit", "exit"): + if len(words) != 1: + raise ValueError("quit takes no values") + return QuitCommand() + if command == "help": + return ModeCommand("help") + if command == "set": + if mode not in ("joints", "cartesian"): + raise ValueError("select a mode first: mode joints or mode cartesian") + command = mode + if command == "joints": + return JointCommand(_numbers(arguments, 7, "joints")) + if command in ("cartesian", "tcp"): + values = _numbers(arguments, 6, "cartesian") + return CartesianCommand(values[:3], values[3:]) + if mode == "joints": + return JointCommand(_numbers(words, 7, "joints")) + if mode == "cartesian": + values = _numbers(words, 6, "cartesian") + return CartesianCommand(values[:3], values[3:]) + raise ValueError("unknown command; type help") + + +def base_pose_to_world(base_position, base_euler, target_position, target_rpy): + """Build a world TCP pose from a target pose expressed in robot-base axes.""" + base = np.eye(4) + base[:3, :3] = Rotation.from_euler("xyz", np.deg2rad(base_euler)).as_matrix() + base[:3, 3] = base_position + target = np.eye(4) + target[:3, :3] = Rotation.from_euler("xyz", target_rpy).as_matrix() + target[:3, 3] = target_position + return base @ target + + +def configure_robot_control(robot, dofs_idx_local): + """Configure stable, bounded PD position control for the seven arm joints.""" + robot.set_dofs_kp(np.array([450., 450., 350., 350., 200., 200., 200.]), + dofs_idx_local=dofs_idx_local) + robot.set_dofs_kv(np.array([45., 45., 35., 35., 20., 20., 20.]), + dofs_idx_local=dofs_idx_local) + robot.set_dofs_force_range( + lower=np.full(7, -200.), upper=np.full(7, 200.), + dofs_idx_local=dofs_idx_local) + + +class TerminalInput: + """Read stdin in a daemon thread so simulation and viewer remain responsive.""" + + def __init__(self, stream=sys.stdin): + self.commands = Queue() + self.stream = stream + self.thread = threading.Thread(target=self._read, daemon=True) + + def start(self): + self.thread.start() + + def _read(self): + for line in self.stream: + self.commands.put(line) + + def poll(self): + try: + return self.commands.get_nowait() + except Empty: + return None + + +def help_text(): + return ( + "Control: mode joints|cartesian; then set ; " + "bare values also work after mode; " + "or use joints / cartesian ; " + "angles radians, position metres, Cartesian frame = robot base; quit" + ) + + +class RobotController: + """Apply joint targets or base-frame TCP targets to a built Genesis robot.""" + + def __init__(self, robot, dofs_idx_local, robot_cfg, tcp_link="link7"): + self.robot = robot + self.dofs_idx_local = dofs_idx_local + self.robot_cfg = robot_cfg + self.tcp_link = tcp_link + self.mode = "joints" + + def apply(self, command): + if isinstance(command, ModeCommand): + if command.mode in ("joints", "cartesian"): + self.mode = command.mode + return + if isinstance(command, JointCommand): + self.mode = "joints" + self.robot.control_dofs_position( + np.asarray(command.angles), dofs_idx_local=self.dofs_idx_local) + return + if isinstance(command, CartesianCommand): + self.mode = "cartesian" + pose = base_pose_to_world( + self.robot_cfg["position"], self.robot_cfg["euler"], + command.position, command.rpy) + quat = Rotation.from_matrix(pose[:3, :3]).as_quat(scalar_first=True) + link = self.robot.get_link(self.tcp_link) + qpos = self.robot.inverse_kinematics( + link=link, pos=pose[:3, 3], quat=quat, + dofs_idx_local=self.dofs_idx_local) + self.robot.control_dofs_position(qpos, dofs_idx_local=self.dofs_idx_local) + return + if isinstance(command, QuitCommand): + return + raise TypeError(f"Unsupported command: {type(command).__name__}") diff --git a/src/iiwa_genesis/control_gui.py b/src/iiwa_genesis/control_gui.py new file mode 100644 index 0000000..e96e299 --- /dev/null +++ b/src/iiwa_genesis/control_gui.py @@ -0,0 +1,183 @@ +"""Tkinter control window kept separate from the Genesis process.""" +import multiprocessing as mp +import math +from queue import Empty + + +def command_from_fields(mode, values, speed_percent): + """Convert GUI strings into a picklable command dictionary.""" + expected = {"joints": 7, "cartesian": 6}.get(mode) + if expected is None: + raise ValueError("mode must be joints or cartesian") + if len(values) != expected: + raise ValueError(f"{mode} expects {expected} values") + try: + numbers = [float(value) for value in values] + speed = float(speed_percent) + except (TypeError, ValueError) as exc: + raise ValueError("GUI values must be numeric") from exc + if not all(math.isfinite(value) for value in numbers + [speed]): + raise ValueError("GUI values must be finite") + if not 0 <= speed <= 100: + raise ValueError("speed_percent must be between 0 and 100") + if speed == 0: + return {"type": "stop"} + return {"type": mode, "values": numbers, "speed_percent": speed} + + +def _observe_parent_close(root, closed): + """Close a Tk root cooperatively when the parent sets its event.""" + if closed.is_set(): + root.destroy() + else: + root.after(50, lambda: _observe_parent_close(root, closed)) + + +def _run_control_window(queue, closed, status, diagnostics): + def fail(message): + status.value = 3 + try: + diagnostics.put({"status": "failed", "message": message}) + except (OSError, ValueError): + pass + + try: + import tkinter as tk + from tkinter import messagebox, ttk + except (ImportError, OSError) as exc: + fail(str(exc)) + closed.set() + return + + try: + status.value = 1 + root = tk.Tk() + root.title("iiwa control") + mode = tk.StringVar(value="joints") + speed = tk.DoubleVar(value=50) + joint_entries = [] + cartesian_entries = [] + + ttk.Label(root, text="Mode").grid(row=0, column=0, sticky="w") + selector = ttk.Combobox(root, textvariable=mode, values=("joints", "cartesian"), + state="readonly", width=12) + selector.grid(row=0, column=1, columnspan=3, sticky="ew") + + ttk.Label(root, text="Joints (rad)").grid(row=1, column=0, sticky="w") + for index in range(7): + entry = ttk.Entry(root, width=9) + entry.insert(0, "0") + entry.grid(row=2, column=index, padx=2, pady=2) + joint_entries.append(entry) + + ttk.Label(root, text="Cartesian (m, rad: x y z roll pitch yaw)").grid( + row=3, column=0, columnspan=7, sticky="w") + for index in range(6): + entry = ttk.Entry(root, width=9) + entry.insert(0, "0") + entry.grid(row=4, column=index, padx=2, pady=2) + cartesian_entries.append(entry) + + ttk.Label(root, text="Speed").grid(row=5, column=0, sticky="w") + ttk.Scale(root, from_=0, to=100, variable=speed, orient="horizontal").grid( + row=5, column=1, columnspan=5, sticky="ew") + ttk.Label(root, textvariable=speed).grid(row=5, column=6) + + def send(): + entries = joint_entries if mode.get() == "joints" else cartesian_entries + try: + command = command_from_fields(mode.get(), [entry.get() for entry in entries], speed.get()) + except ValueError as exc: + messagebox.showerror("Invalid command", str(exc), parent=root) + return + try: + queue.put(command) + except (OSError, ValueError): + closed.set() + root.destroy() + + def stop(): + try: + queue.put({"type": "stop"}) + except (OSError, ValueError): + closed.set() + root.destroy() + + def close(): + try: + queue.put({"type": "close"}) + except (OSError, ValueError): + pass + closed.set() + root.destroy() + + ttk.Button(root, text="Send", command=send).grid(row=6, column=0, columnspan=2, pady=6) + ttk.Button(root, text="Stop", command=stop).grid(row=6, column=2, columnspan=2, pady=6) + ttk.Button(root, text="Close", command=close).grid(row=6, column=4, columnspan=2, pady=6) + root.protocol("WM_DELETE_WINDOW", close) + root.after(50, lambda: _observe_parent_close(root, closed)) + root.mainloop() + except tk.TclError as exc: + fail(str(exc)) + finally: + closed.set() + if status.value != 3: + status.value = 2 + + +class ControlGui: + """Own the independent Tkinter process and its command queue.""" + + def __init__(self): + context = mp.get_context("spawn") + self.queue = context.Queue() + self.commands = self.queue + self.closed = context.Event() + self._status = context.Value("i", 0) + self.diagnostics = context.Queue() + self.process = context.Process(target=_run_control_window, + args=(self.queue, self.closed, self._status, self.diagnostics), + daemon=True) + self._started = False + + def start(self): + try: + self.process.start() + self._started = True + except (OSError, RuntimeError): + self._status.value = 3 + self.closed.set() + return self + + @property + def status(self): + return {0: "created", 1: "running", 2: "closed", 3: "failed"}.get( + self._status.value, "failed") + + def poll(self): + try: + return self.queue.get(timeout=0.05) + except (Empty, OSError, ValueError): + return None + + def poll_diagnostic(self): + try: + return self.diagnostics.get(timeout=0.05) + except (Empty, OSError, ValueError): + return None + + def close(self): + self.closed.set() + if self._started and self.process.is_alive(): + self.process.join(timeout=2) + if self._started and self.process.is_alive(): + self.process.terminate() + self.process.join(timeout=2) + self.queue.cancel_join_thread() + self.queue.close() + self.diagnostics.cancel_join_thread() + self.diagnostics.close() + self._status.value = 2 if self.status != "failed" else 3 + + +GuiControl = ControlGui diff --git a/src/iiwa_genesis/scene.py b/src/iiwa_genesis/scene.py index 992eb2b..3f9031e 100644 --- a/src/iiwa_genesis/scene.py +++ b/src/iiwa_genesis/scene.py @@ -1,11 +1,159 @@ +from pathlib import Path +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + from .camera import D455 from .config import DESCRIPTION +from .control import ( + CartesianCommand, + JointCommand, + ModeCommand, + VelocityLimitedMotion, + base_pose_to_world, + configure_robot_control, +) + +IK_POSITION_TOLERANCE = 5e-4 +IK_ROTATION_TOLERANCE = 5e-3 +# Genesis returns float32 joint values; allow sub-microradian boundary noise. +IK_JOINT_LIMIT_TOLERANCE = 1e-6 + + +def load_joint_limits(urdf_path): + """Read position and velocity limits in joint1 through joint7 order.""" + joints = { + joint.get("name"): joint + for joint in ET.parse(Path(urdf_path)).getroot().findall("joint") + if joint.get("type") == "revolute" + } + try: + limits = np.asarray([ + [float(joints[f"joint{index}"].find("limit").get(name)) + for index in range(1, 8)] + for name in ("lower", "upper", "velocity") + ]) + except (AttributeError, KeyError, TypeError, ValueError) as exc: + raise ValueError("URDF must define lower, upper and velocity limits for joint1 through joint7") from exc + lower, upper, velocity = limits + if not np.isfinite(limits).all() or (lower > upper).any() or (velocity <= 0).any(): + raise ValueError("URDF limits must be finite, ordered and have positive velocities") + return lower, upper, velocity + + +def load_joint_velocity_limits(urdf_path): + return load_joint_limits(urdf_path)[2] + + +def _as_numpy(value): + """Move a Genesis tensor to CPU before converting to NumPy.""" + detach = getattr(value, "detach", None) + if detach is not None: + value = detach() + cpu = getattr(value, "cpu", None) + if cpu is not None: + value = cpu() + numpy = getattr(value, "numpy", None) + if numpy is not None: + value = numpy() + return np.asarray(value, dtype=float) + + +def _as_joint_vector(value): + result = _as_numpy(value) + if result.shape != (7,) or not np.isfinite(result).all(): + raise ValueError("Genesis returned an invalid seven-joint target") + return result + + +class SceneMotionController: + """Plan interactive commands and send one velocity-limited target per step.""" + + def __init__(self, robot, dofs_idx_local, cfg): + self.robot = robot + self.dofs_idx_local = list(dofs_idx_local) + self.robot_cfg = cfg["robot"] + self.tcp_link = "tcp" if cfg["scene"]["mode"] == "with_gripper" else "link7" + self.lower, self.upper, velocity = load_joint_limits(self.robot_cfg["urdf"]) + self.motion = VelocityLimitedMotion(velocity) + self.mode = "joints" + self.dt = cfg["scene"]["dt"] + self.commanded_q = self.current_q().copy() + self.motion.stop(self.commanded_q) + + def current_q(self): + return _as_joint_vector(self.robot.get_dofs_position(dofs_idx_local=self.dofs_idx_local)) + + def validate_target(self, value, tolerance=0): + target = _as_joint_vector(value) + if ((target < self.lower - tolerance) | (target > self.upper + tolerance)).any(): + raise ValueError("Joint target is outside URDF position limits") + return np.clip(target, self.lower, self.upper) + + def apply(self, command, speed_percent=100): + if isinstance(command, ModeCommand): + if command.mode in ("joints", "cartesian"): + self.mode = command.mode + return + if isinstance(command, JointCommand): + self.motion.set_target(self.validate_target(command.angles), speed_percent) + self.mode = "joints" + return + if isinstance(command, CartesianCommand): + pose = base_pose_to_world( + self.robot_cfg["position"], self.robot_cfg["euler"], + command.position, command.rpy) + quat = Rotation.from_matrix(pose[:3, :3]).as_quat(scalar_first=True) + try: + qpos, residual = self.robot.inverse_kinematics( + link=self.robot.get_link(self.tcp_link), pos=pose[:3, 3], quat=quat, + dofs_idx_local=self.dofs_idx_local, return_error=True, + pos_tol=IK_POSITION_TOLERANCE, rot_tol=IK_ROTATION_TOLERANCE) + except Exception as exc: + # GenesisException derives directly from Exception. + raise ValueError(f"Cartesian IK failed: {exc}") from exc + residual = _as_numpy(residual) + if (residual.shape != (6,) or not np.isfinite(residual).all() + or np.max(np.abs(residual[:3])) > IK_POSITION_TOLERANCE + or np.max(np.abs(residual[3:])) > IK_ROTATION_TOLERANCE): + raise ValueError( + "Cartesian IK did not converge within pose tolerances " + f"(position={residual[:3]}, rotation={residual[3:]})") + self.motion.set_target( + self.validate_target(qpos, IK_JOINT_LIMIT_TOLERANCE), speed_percent) + self.mode = "cartesian" + return + raise TypeError(f"Unsupported command: {type(command).__name__}") + + def apply_gui(self, command): + command_type = command.get("type") + if command_type == "stop": + self.motion.stop(self.commanded_q) + return + if command_type == "joints": + self.apply(JointCommand(tuple(command["values"])), command["speed_percent"]) + return + if command_type == "cartesian": + values = command["values"] + self.apply(CartesianCommand(tuple(values[:3]), tuple(values[3:])), + command["speed_percent"]) + return + if command_type == "close": + return + raise ValueError("unknown GUI command") + + def step(self): + position, complete = self.motion.step(self.commanded_q, self.dt) + self.robot.control_dofs_position(position, dofs_idx_local=self.dofs_idx_local) + self.commanded_q = position.copy() + return complete def build_scene(gs, cfg, viewer): scene = gs.Scene( show_viewer=viewer, - # v1 is a pose inspection scene: no falling arm and no active controller. + # Zero gravity supports pose inspection with bounded position control. sim_options=gs.options.SimOptions(dt=cfg["scene"]["dt"], gravity=(0, 0, 0)), viewer_options=gs.options.ViewerOptions(camera_pos=(2.8, -3, 2.6), camera_lookat=(0, 0, 1.1)), vis_options=gs.options.VisOptions(show_world_frame=True), @@ -20,8 +168,6 @@ def build_scene(gs, cfg, viewer): robot = entity # Reuse the supplied D455 CAD mesh. Its lens face is at mesh Z=0; body extends +Z. from .camera import optical_pose - from scipy.spatial.transform import Rotation - import numpy as np overhead = cfg["cameras"]["overhead"] pose = optical_pose(overhead["position"], overhead["lookat"], overhead["up"]) mesh_pos = pose[:3, 3] + pose[:3, :3] @ [-0.062, 0.0155, -0.001] @@ -35,5 +181,6 @@ def build_scene(gs, cfg, viewer): indices = [robot.get_joint(f"joint{i}").dofs_idx_local[0] for i in range(1, 8)] if robot.n_dofs != 7: raise ValueError(f"Expected seven robot DOFs, got {robot.n_dofs}") + configure_robot_control(robot, indices) robot.set_dofs_position([cfg["robot"]["joints"][f"joint{i}"] for i in range(1, 8)], dofs_idx_local=indices) return scene, robot, cameras diff --git a/tests/test_control.py b/tests/test_control.py new file mode 100644 index 0000000..afc63d0 --- /dev/null +++ b/tests/test_control.py @@ -0,0 +1,800 @@ +import numpy as np +import pytest +import pickle +from multiprocessing import get_context +from queue import Empty, Queue +from types import SimpleNamespace +import xml.etree.ElementTree as ET + +from iiwa_genesis.control import ( + CartesianCommand, + JointCommand, + RobotController, + VelocityLimitedMotion, + configure_robot_control, + base_pose_to_world, + parse_command, + GuiCommandQueue, +) +from iiwa_genesis.control_gui import ControlGui, _observe_parent_close, command_from_fields +from iiwa_genesis.scene import SceneMotionController + + +IIWA_VELOCITY_LIMITS = (1.71, 1.71, 1.75, 2.27, 2.44, 3.14, 3.14) + + +class FakeRobot: + def __init__(self): + self.calls = [] + self.position = np.zeros(7) + + def control_dofs_position(self, position, dofs_idx_local=None): + self.position = np.asarray(position, dtype=float) + self.calls.append(("control", self.position.copy(), dofs_idx_local)) + + def get_dofs_position(self, dofs_idx_local=None): + self.calls.append(("position", dofs_idx_local)) + return self.position + + def get_link(self, name): + self.calls.append(("link", name)) + return name + + def inverse_kinematics(self, **kwargs): + self.calls.append(("ik", kwargs)) + qpos = np.arange(7, dtype=float) * .2 + return (qpos, np.zeros(6)) if kwargs.get("return_error") else qpos + + def set_dofs_kp(self, kp, dofs_idx_local=None): + self.calls.append(("kp", np.asarray(kp), dofs_idx_local)) + + def set_dofs_kv(self, kv, dofs_idx_local=None): + self.calls.append(("kv", np.asarray(kv), dofs_idx_local)) + + def set_dofs_force_range(self, lower, upper, dofs_idx_local=None): + self.calls.append(("force", np.asarray(lower), np.asarray(upper), dofs_idx_local)) + + +def test_parses_joint_mode_and_seven_angles(): + command = parse_command("joints 0 0.2 0 1.0 0 -0.3 0.1") + assert isinstance(command, JointCommand) + np.testing.assert_allclose(command.angles, [0, 0.2, 0, 1.0, 0, -0.3, 0.1]) + + +def test_set_uses_all_seven_values_after_selecting_joints_mode(): + command = parse_command("set 0 0.2 0 1.0 0 0 0", mode="joints") + assert isinstance(command, JointCommand) + np.testing.assert_allclose(command.angles, [0, 0.2, 0, 1.0, 0, 0, 0]) + + +def test_selected_joints_mode_accepts_bare_angle_values(): + command = parse_command("0 0.2 0 1.0 0 0 0", mode="joints") + assert isinstance(command, JointCommand) + np.testing.assert_allclose(command.angles, [0, 0.2, 0, 1.0, 0, 0, 0]) + + +def test_parses_cartesian_tcp_pose_in_robot_base_frame(): + command = parse_command("cartesian 0.35 0 0.55 0 1.57 0") + assert isinstance(command, CartesianCommand) + np.testing.assert_allclose(command.position, [0.35, 0, 0.55]) + np.testing.assert_allclose(command.rpy, [0, 1.57, 0]) + + +def test_cartesian_target_is_transformed_from_robot_base_to_world(): + pose = base_pose_to_world([1, 2, 3], [0, 0, 90], [0.5, 0, 0.2], [0, 0, 0]) + np.testing.assert_allclose(pose[:3, 3], [1, 2.5, 3.2], atol=1e-7) + + +@pytest.mark.parametrize("line", ["joints 0 0 0", "cartesian 0 0 0 0", "mode unknown"]) +def test_rejects_invalid_commands(line): + with pytest.raises(ValueError): + parse_command(line) + + +def test_controller_sends_joint_target_to_position_controller(): + robot = FakeRobot() + controller = RobotController(robot, [1, 2, 3, 4, 5, 6, 7], + {"position": [1, 2, 3], "euler": [0, 0, 0]}) + controller.apply(JointCommand((0, 1, 2, 3, 4, 5, 6))) + assert robot.calls[0][0] == "control" + np.testing.assert_array_equal(robot.calls[0][1], [0, 1, 2, 3, 4, 5, 6]) + + +def test_configures_position_controller_for_all_arm_dofs(): + robot = FakeRobot() + configure_robot_control(robot, [1, 2, 3, 4, 5, 6, 7]) + assert [call[0] for call in robot.calls] == ["kp", "kv", "force"] + + +def test_controller_sends_cartesian_target_to_ik_in_world_frame(): + robot = FakeRobot() + controller = RobotController(robot, list(range(7)), + {"position": [1, 2, 3], "euler": [0, 0, 0]}, "tcp") + controller.apply(CartesianCommand((0.5, 0, 0.2), (0, 0, 0))) + ik = next(call[1] for call in robot.calls if call[0] == "ik") + np.testing.assert_allclose(ik["pos"], [1.5, 2, 3.2]) + assert ik["link"] == "tcp" + + +def test_cartesian_ik_accepts_componentwise_genesis_tolerances(): + robot = FakeRobot() + robot.inverse_kinematics = lambda **kwargs: (np.zeros(7), np.full(6, 0.0004)) + controller = SceneMotionController(robot, list(range(7)), motion_config()) + controller.apply(CartesianCommand((0.5, 0, 0.2), (0, 0, 0)), 50) + + +def test_scene_motion_controller_plans_gui_cartesian_command_at_selected_speed(): + robot = FakeRobot() + controller = SceneMotionController( + robot, + list(range(7)), + { + "scene": {"mode": "with_gripper", "dt": 0.1}, + "robot": { + "urdf": "iiwa_genesis_description/urdf/iiwa_with_gripper.urdf", + "position": [1, 2, 3], + "euler": [0, 0, 0], + }, + }, + ) + + controller.apply_gui({ + "type": "cartesian", "values": [0.5, 0, 0.2, 0, 0, 0], + "speed_percent": 50, + }) + controller.step() + + ik = next(call[1] for call in robot.calls if call[0] == "ik") + np.testing.assert_allclose(ik["pos"], [1.5, 2, 3.2]) + assert ik["link"] == "tcp" + np.testing.assert_allclose( + robot.calls[-1][1], [0, .0855, .0875, .1135, .122, .157, .157]) + + +class LaggingRobot(FakeRobot): + def control_dofs_position(self, position, dofs_idx_local=None): + self.calls.append(("control", np.asarray(position).copy(), dofs_idx_local)) + + +def motion_config(mode="with_gripper"): + return { + "scene": {"mode": mode, "dt": .1}, + "robot": { + "urdf": f"iiwa_genesis_description/urdf/iiwa_{mode}.urdf", + "position": [1, 2, 3], "euler": [0, 0, 90], + }, + } + + +@pytest.mark.parametrize("mode,link", [("with_gripper", "tcp"), ("without_gripper", "link7")]) +def test_queued_cartesian_motion_advances_despite_lagging_feedback(mode, link): + robot = LaggingRobot() + controller = SceneMotionController(robot, list(range(7)), motion_config(mode)) + commands = Queue() + commands.put({"type": "cartesian", "values": [.5, 0, .2, 0, 0, 0], + "speed_percent": 50}) + controller.apply_gui(commands.get_nowait()) + for _ in range(3): + controller.step() + targets = [call[1] for call in robot.calls if call[0] == "control"] + np.testing.assert_allclose(targets[1], [0, .171, .175, .227, .244, .314, .314]) + np.testing.assert_allclose(targets[2], [0, .2, .2625, .3405, .366, .471, .471]) + ik = next(call[1] for call in robot.calls if call[0] == "ik") + assert ik["link"] == link + assert ik["return_error"] is True + assert ik["pos_tol"] == .0005 + assert ik["rot_tol"] == .005 + np.testing.assert_allclose(ik["pos"], [1, 2.5, 3.2]) + np.testing.assert_allclose(ik["quat"], [2**-.5, 0, 0, 2**-.5], atol=1e-7) + + +def test_queued_stop_freezes_commanded_target_and_next_move_starts_there(): + robot = LaggingRobot() + controller = SceneMotionController(robot, list(range(7)), motion_config()) + commands = Queue() + commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50}) + controller.apply_gui(commands.get_nowait()) + controller.step() + controller.step() + frozen = np.array([.171, .171, .175, .227, .244, .314, .314]) + commands.put({"type": "stop"}) + controller.apply_gui(commands.get_nowait()) + for _ in range(3): + assert controller.step() is True + np.testing.assert_allclose(robot.calls[-1][1], frozen) + controller.apply(JointCommand((0,) * 7)) + controller.step() + np.testing.assert_allclose(robot.calls[-1][1], np.zeros(7), atol=1e-15) + + +@pytest.mark.parametrize("target", [[4] * 7, [-4] * 7, [0] * 6, [np.nan] * 7, [np.inf] * 7]) +@pytest.mark.parametrize("mode", ["with_gripper", "without_gripper"]) +def test_invalid_joint_target_preserves_active_motion(target, mode): + robot = LaggingRobot() + controller = SceneMotionController(robot, list(range(7)), motion_config(mode)) + controller.apply(JointCommand((1,) * 7), 50) + controller.step() + with pytest.raises(ValueError): + controller.apply_gui({"type": "joints", "values": target, "speed_percent": 100}) + controller.step() + np.testing.assert_allclose(robot.calls[-1][1], [.171, .171, .175, .227, .244, .314, .314]) + + +@pytest.mark.parametrize("index", range(7)) +@pytest.mark.parametrize("boundary", [-.25, .5]) +def test_selected_urdf_position_limits_apply_to_each_joint(tmp_path, index, boundary): + cfg = motion_config() + tree = ET.parse(cfg["robot"]["urdf"]) + limit = tree.find(f"joint[@name='joint{index + 1}']/limit") + limit.set("lower", "-.25") + limit.set("upper", ".5") + cfg["robot"]["urdf"] = tmp_path / "robot.urdf" + tree.write(cfg["robot"]["urdf"]) + robot = LaggingRobot() + controller = SceneMotionController(robot, list(range(7)), cfg) + target = np.zeros(7) + target[index] = boundary + controller.apply(JointCommand(tuple(target))) + for _ in range(4): + controller.step() + np.testing.assert_allclose(robot.calls[-1][1], target) + invalid = target.copy() + invalid[index] += -.001 if boundary < 0 else .001 + with pytest.raises(ValueError, match="URDF"): + controller.apply(JointCommand(tuple(invalid))) + assert controller.step() is True + np.testing.assert_allclose(robot.calls[-1][1], target) + + +@pytest.mark.parametrize("result", [ + (np.zeros(7), [.0006, 0, 0, 0, 0, 0]), + (np.zeros(7), [0, 0, 0, .006, 0, 0]), + (np.zeros(7), [np.nan] * 6), + (np.zeros(7), [np.inf] * 6), + (np.zeros(7), [0] * 5), + (np.full(7, 4), np.zeros(6)), + (np.full(7, np.nan), np.zeros(6)), + Exception("Genesis solver failed"), +]) +def test_failed_ik_is_recoverable_and_preserves_active_motion(result, monkeypatch): + robot = LaggingRobot() + controller = SceneMotionController(robot, list(range(7)), motion_config()) + controller.apply(JointCommand((1,) * 7), 50) + controller.step() + + def inverse_kinematics(**kwargs): + if isinstance(result, Exception): + raise result + return result + + monkeypatch.setattr(robot, "inverse_kinematics", inverse_kinematics) + with pytest.raises(ValueError): + controller.apply(CartesianCommand((100, 0, 0), (0, 0, 0))) + assert controller.mode == "joints" + controller.step() + np.testing.assert_allclose(robot.calls[-1][1], [.171, .171, .175, .227, .244, .314, .314]) + + +@pytest.fixture +def cli_runtime(monkeypatch, tmp_path): + from iiwa_genesis import cli, control_gui, preview, runtime, scene + + robot = LaggingRobot() + robot.get_joint = lambda name: SimpleNamespace(dofs_idx_local=[int(name[-1]) - 1]) + terminal_queue = Queue() + gui = SimpleNamespace(commands=Queue(), diagnostics=Queue(), status="running") + events = [] + scheduled = {} + + def poll_terminal(): + try: + return terminal_queue.get_nowait() + except Empty: + return None + + terminal = SimpleNamespace(start=lambda: None, poll=poll_terminal) + gui.start = lambda: gui + gui.close = lambda: events.append("gui closed") + fake_preview = SimpleNamespace(close=lambda: events.append("preview closed"), + stop=SimpleNamespace(is_set=lambda: False), + submit=lambda *args: None) + steps = [] + + def step(): + steps.append(robot.calls[-1][1].copy()) + if len(steps) in scheduled: + scheduled[len(steps)]() + + fake_scene = SimpleNamespace(step=step, viewer=SimpleNamespace(is_alive=lambda: True)) + camera = SimpleNamespace(name="overhead", cfg={"fps": 10, "max_range": 6}, + read=lambda timestamp: events.append(("frame", timestamp))) + gs = SimpleNamespace(cpu="cpu", backend="cpu", __version__="fake", + init=lambda **kwargs: None, destroy=lambda: events.append("destroy")) + cfg = motion_config() + cfg["scene"].update(viewer=False, backend="cpu", seed=0) + cfg["output"] = dict(save_frames=False, preview=True, directory=tmp_path, pointcloud=False) + monkeypatch.setattr(cli, "load_config", lambda path: cfg) + monkeypatch.setattr(runtime, "import_genesis", lambda: gs) + monkeypatch.setattr(scene, "build_scene", lambda *args: (fake_scene, robot, [camera])) + monkeypatch.setattr(cli, "TerminalInput", lambda: terminal) + monkeypatch.setattr(control_gui, "ControlGui", lambda: gui) + monkeypatch.setattr(preview, "Preview", lambda: fake_preview) + monkeypatch.setattr(cli.time, "sleep", lambda duration: None) + monkeypatch.setattr("sys.argv", ["run_scene.py", "--viewer", "--steps", "3", + "--interactive", "--control-window"]) + return SimpleNamespace(run=cli.main, robot=robot, gui=gui, terminal=terminal_queue, + events=events, steps=steps, scheduled=scheduled, preview=fake_preview) + + +@pytest.mark.parametrize("mode", ["with_gripper", "without_gripper"]) +@pytest.mark.parametrize("side", ["lower", "upper"]) +@pytest.mark.parametrize("outward", [False, True]) +def test_cartesian_ik_clips_float32_joint_boundaries(mode, side, outward, monkeypatch): + robot = LaggingRobot() + controller = SceneMotionController(robot, list(range(7)), motion_config(mode)) + boundary = getattr(controller, side).copy() + result = boundary.astype(np.float32) + if outward: + result = np.nextafter(result, np.float32(-np.inf if side == "lower" else np.inf)) + monkeypatch.setattr(robot, "inverse_kinematics", lambda **kwargs: (result, np.zeros(6))) + + controller.apply(CartesianCommand((.5, 0, .2), (0, 0, 0))) + for _ in range(40): + controller.step() + + expected = np.array([2.97, 2.09, 2.97, 2.09, 2.97, 2.09, 3.05] if outward else [ + 2.97, 2.0899999141693115, 2.97, 2.0899999141693115, + 2.97, 2.0899999141693115, 3.049999952316284]) + if side == "lower": + expected = -expected + np.testing.assert_array_equal(robot.calls[-1][1], expected) + assert (robot.calls[-1][1] >= controller.lower).all() + assert (robot.calls[-1][1] <= controller.upper).all() + np.testing.assert_array_equal(getattr(controller, side), boundary) + + +@pytest.mark.parametrize("index", range(7)) +@pytest.mark.parametrize("side,offset", [("lower", -1e-5), ("upper", 1e-5)]) +def test_cartesian_ik_rejects_real_boundary_violation(index, side, offset, monkeypatch): + robot = LaggingRobot() + controller = SceneMotionController(robot, list(range(7)), motion_config()) + controller.apply(JointCommand((1,) * 7), 50) + controller.step() + result = np.zeros(7) + result[index] = getattr(controller, side)[index] + offset + monkeypatch.setattr(robot, "inverse_kinematics", lambda **kwargs: (result, np.zeros(6))) + + with pytest.raises(ValueError, match="URDF"): + controller.apply(CartesianCommand((.5, 0, .2), (0, 0, 0))) + assert controller.mode == "joints" + controller.step() + np.testing.assert_allclose(robot.calls[-1][1], [.171, .171, .175, .227, .244, .314, .314]) + + +class SustainedQueue: + def __init__(self, app, value): + self.app = app + self.value = value + self.reads = {} + + def get_nowait(self): + tick = len(self.app.steps) + self.reads[tick] = self.reads.get(tick, 0) + 1 + assert self.reads[tick] < 1000, "input processing starved physics" + return self.value + + +@pytest.mark.parametrize("channel", ["terminal", "commands", "diagnostics"]) +def test_cli_sustained_input_yields_to_physics_and_frames(cli_runtime, channel, monkeypatch): + from iiwa_genesis import cli + + app = cli_runtime + value = "joints 1 1 1 1 1 1 1" if channel == "terminal" else { + "type": "joints", "values": [1] * 7, "speed_percent": 100} + queue = SustainedQueue(app, value) + if channel == "terminal": + monkeypatch.setattr(cli, "TerminalInput", lambda: SimpleNamespace( + start=lambda: None, poll=queue.get_nowait)) + else: + setattr(app.gui, channel, queue) + app.terminal.put("joints 1 1 1 1 1 1 1") + + app.run() + + assert len(app.steps) == 3 + assert set(queue.reads) == {0, 1, 2} + assert len([event for event in app.events if isinstance(event, tuple)]) == 3 + assert (app.steps[-1] > app.steps[0]).all() + + +@pytest.mark.parametrize("quit_line", ["quit", " EXIT "]) +def test_cli_quit_exits_before_pending_movement_gui_or_physics(cli_runtime, quit_line): + app = cli_runtime + app.terminal.put("joints 1 1 1 1 1 1 1") + + def request_quit(): + app.terminal.put("cartesian .5 0 .2 0 0 0") + app.terminal.put(quit_line) + app.gui.commands.put({"type": "joints", "values": [-1] * 7, "speed_percent": 100}) + app.gui.diagnostics.put({"message": "pending diagnostic"}) + + app.scheduled[1] = request_quit + app.run() + + assert len(app.steps) == 1 + assert not any(call[0] == "ik" for call in app.robot.calls) + assert app.gui.commands.qsize() == 1 + assert app.gui.diagnostics.qsize() == 1 + assert len([event for event in app.events if isinstance(event, tuple)]) == 1 + assert app.events[-3:] == ["gui closed", "preview closed", "destroy"] + + +def test_cli_stop_preempts_pending_ik_from_both_inputs_and_allows_later_move(cli_runtime): + app = cli_runtime + app.terminal.put("cartesian .5 0 .2 0 0 0") + app.gui.commands.put({"type": "cartesian", "values": [.5, 0, .2, 0, 0, 0], + "speed_percent": 100}) + app.gui.commands.put({"type": "stop"}) + app.scheduled[1] = lambda: app.gui.commands.put({ + "type": "joints", "values": [1] * 7, "speed_percent": 50}) + app.run() + + assert not any(call[0] == "ik" for call in app.robot.calls) + np.testing.assert_array_equal(app.steps[0], np.zeros(7)) + np.testing.assert_allclose(app.steps[1], [.0855, .0855, .0875, .1135, .122, .157, .157]) + + +def test_cli_stop_defers_already_queued_later_move_until_next_tick(cli_runtime): + app = cli_runtime + app.gui.commands.put({"type": "stop"}) + app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50}) + app.run() + + np.testing.assert_array_equal(app.steps[0], np.zeros(7)) + np.testing.assert_allclose(app.steps[1], [.0855, .0855, .0875, .1135, .122, .157, .157]) + np.testing.assert_allclose(app.steps[2], [.171, .171, .175, .227, .244, .314, .314]) + + +@pytest.mark.parametrize("source", ["terminal", "gui"]) +def test_cli_bounded_batches_preserve_backlogged_commands(cli_runtime, source, monkeypatch): + app = cli_runtime + for _ in range(100): + if source == "terminal": + app.terminal.put("help") + else: + app.gui.commands.put({"type": "close"}) + if source == "terminal": + app.terminal.put("joints 1 1 1 1 1 1 1") + else: + app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 100}) + + # Run enough ticks to reach the final command while checking frames continue. + monkeypatch.setattr("sys.argv", ["run_scene.py", "--viewer", "--steps", "12", + "--interactive", "--control-window"]) + app.run() + + np.testing.assert_array_equal(app.steps[0], np.zeros(7)) + assert (app.steps[-1] > 0).all() + assert len(app.steps) == 12 + + +def test_cli_batch_parses_set_after_mode_change_and_invalid_quit(cli_runtime, capsys): + app = cli_runtime + app.terminal.put("quit extra") + app.terminal.put("mode cartesian") + app.terminal.put("set .5 0 .2 0 0 0") + app.run() + + assert len(app.steps) == 3 + assert len([call for call in app.robot.calls if call[0] == "ik"]) == 1 + assert "quit takes no values" in capsys.readouterr().out + + +@pytest.mark.parametrize("side,offset", [("lower", -1e-7), ("upper", 1e-7)]) +def test_joint_commands_keep_strict_limits_for_tiny_excursions(side, offset): + robot = LaggingRobot() + controller = SceneMotionController(robot, list(range(7)), motion_config()) + target = getattr(controller, side) + offset + with pytest.raises(ValueError, match="URDF"): + controller.apply(JointCommand(tuple(target))) + + +class BrokenQueue: + def __init__(self, error): + self.error = error + self.reads = 0 + + def get_nowait(self): + self.reads += 1 + raise self.error("queue disconnected") + + +@pytest.mark.parametrize("channel", ["commands", "diagnostics"]) +@pytest.mark.parametrize("error", [EOFError, OSError, ValueError]) +def test_cli_disables_only_broken_gui_queue_and_keeps_terminal_and_frames(cli_runtime, channel, error, capsys): + app = cli_runtime + broken = BrokenQueue(error) + setattr(app.gui, channel, broken) + app.terminal.put("joints 1 1 1 1 1 1 1") + app.scheduled[1] = lambda: app.terminal.put("joints -1 -1 -1 -1 -1 -1 -1") + if channel == "diagnostics": + app.scheduled[2] = lambda: app.gui.commands.put({"type": "stop"}) + else: + app.gui.diagnostics.put({"message": "child startup failed"}) + app.run() + assert broken.reads == 1 + assert len(app.steps) == 3 + np.testing.assert_allclose(app.steps[1], np.zeros(7), atol=1e-15) + if channel == "diagnostics": + np.testing.assert_allclose(app.steps[2], app.steps[1]) + else: + assert (app.steps[2] < 0).all() + assert len([event for event in app.events if isinstance(event, tuple)]) == 3 + assert app.events[-3:] == ["gui closed", "preview closed", "destroy"] + assert "queue disconnected" in capsys.readouterr().out + + +def test_cli_rejects_gui_command_but_processes_following_valid_command(cli_runtime, capsys): + app = cli_runtime + app.gui.commands.put({"type": "joints", "values": [4] * 7, "speed_percent": 100}) + app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50}) + app.run() + np.testing.assert_allclose(app.steps[0], [.0855, .0855, .0875, .1135, .122, .157, .157]) + np.testing.assert_allclose(app.steps[2], [.2565, .2565, .2625, .3405, .366, .471, .471]) + assert "Command error" in capsys.readouterr().out + + +@pytest.mark.parametrize("source", ["terminal", "gui"]) +def test_cli_recovers_from_genesis_ik_exception_without_losing_motion(cli_runtime, source, monkeypatch, capsys): + app = cli_runtime + + def fail(**kwargs): + raise Exception("Genesis IK failure") + + monkeypatch.setattr(app.robot, "inverse_kinematics", fail) + app.gui.commands.put({"type": "joints", "values": [1] * 7, "speed_percent": 50}) + if source == "terminal": + app.scheduled[1] = lambda: app.terminal.put("cartesian 100 0 0 0 0 0") + else: + app.scheduled[1] = lambda: app.gui.commands.put({ + "type": "cartesian", "values": [100, 0, 0, 0, 0, 0], "speed_percent": 100}) + app.scheduled[2] = lambda: app.gui.commands.put({"type": "stop"}) + app.run() + np.testing.assert_allclose(app.steps[1], [.171, .171, .175, .227, .244, .314, .314]) + np.testing.assert_allclose(app.steps[2], app.steps[1]) + assert app.events[-1] == "destroy" + output = capsys.readouterr().out + assert "Command error" in output + assert "Genesis IK failure" in output + + +@pytest.mark.parametrize("failure", ["startup", "close", "preview_close"]) +def test_cli_gui_failures_always_destroy_genesis(cli_runtime, failure, capsys): + app = cli_runtime + + def fail(): + raise RuntimeError(failure) + + if failure == "startup": + app.gui.start = fail + elif failure == "close": + app.gui.close = fail + else: + app.preview.close = fail + app.terminal.put("joints 1 1 1 1 1 1 1") + app.run() + assert len(app.steps) == 3 + assert app.events[-1] == "destroy" + if failure != "preview_close": + assert "preview closed" in app.events + assert failure in capsys.readouterr().out + + +def test_velocity_limited_motion_scales_each_urdf_velocity_limit(): + motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS) + + motion.set_target(np.full(7, 10.0), 50) + position, complete = motion.step(np.zeros(7), 1.0) + + np.testing.assert_allclose(position, np.asarray(IIWA_VELOCITY_LIMITS) * 0.5) + assert complete is False + + +@pytest.mark.parametrize( + "speed_percent", [np.nan, np.inf, -1, 0, 101, "50", "fast", True, + np.bool_(False), 1 + 0j, None]) +def test_velocity_limited_motion_rejects_invalid_speed(speed_percent): + motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS) + + with pytest.raises(ValueError): + motion.set_target(np.ones(7), speed_percent) + + +def test_velocity_limited_motion_rejects_numeric_string_speed_percent(): + motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS) + + with pytest.raises(ValueError): + motion.set_target(np.full(7, 10.0), "50") + + +@pytest.mark.parametrize( + "dt", ["1", "fast", True, np.bool_(False), 1 + 0j, None, np.nan, + np.inf, -1]) +def test_velocity_limited_motion_rejects_invalid_dt(dt): + motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS) + motion.set_target(np.ones(7), 50) + + with pytest.raises(ValueError): + motion.step(np.zeros(7), dt) + + +def test_velocity_limited_motion_applies_mixed_joint_caps_independently(): + limits = np.asarray([0.5, 2.0, 1.0, 3.0, 0.25, 4.0, 1.5]) + motion = VelocityLimitedMotion(limits) + motion.set_target(np.full(7, 10.0), 50) + + position, complete = motion.step(np.zeros(7), 1.0) + + np.testing.assert_allclose(position, limits * 0.5) + assert complete is False + + +def test_velocity_limited_motion_clamps_each_joint_and_reports_completion(): + motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS) + target = np.asarray([1, -1, 0.1, -0.1, 0.01, -0.01, 0.001]) + motion.set_target(target, 100) + + position, complete = motion.step(np.zeros(7), 10.0) + + np.testing.assert_allclose(position, target) + assert complete is True + assert motion.complete is True + + +def test_velocity_limited_motion_stop_holds_current_position(): + motion = VelocityLimitedMotion(IIWA_VELOCITY_LIMITS) + current = np.arange(7, dtype=float) + motion.set_target(np.ones(7), 50) + + motion.stop(current) + position, complete = motion.step(current + 10, 1.0) + + np.testing.assert_allclose(position, current) + assert complete is True + + +def test_gui_queue_sends_picklable_plain_joint_command_data(): + queue = GuiCommandQueue(get_context("spawn").Queue()) + + queue.send_joint([0, 1, 2, 3, 4, 5, 6], 40) + command = queue.poll() + + assert isinstance(command, dict) + assert command == {"type": "joints", "values": [0., 1., 2., 3., 4., 5., 6.], + "speed_percent": 40.} + assert pickle.loads(pickle.dumps(command)) == command + queue.close() + + +def test_gui_queue_sends_stop_and_close_commands(): + queue = GuiCommandQueue(get_context("spawn").Queue()) + + queue.send_stop() + queue.send_close() + + assert queue.poll() == {"type": "stop"} + assert queue.poll() == {"type": "close"} + assert queue.poll() is None + queue.close() + + +def test_gui_command_from_fields_validates_mode_specific_field_counts(): + command = command_from_fields("cartesian", [".1", ".2", ".3", "0", "1", "2"], "75") + + assert command == {"type": "cartesian", "values": [.1, .2, .3, 0., 1., 2.], + "speed_percent": 75.} + with pytest.raises(ValueError): + command_from_fields("joints", ["0"] * 6, "50") + with pytest.raises(ValueError): + command_from_fields("other", ["0"] * 7, "50") + + +def test_gui_zero_speed_send_becomes_stop_command(): + assert command_from_fields("joints", ["0"] * 7, "0") == {"type": "stop"} + + +@pytest.mark.parametrize("speed", ["nan", "inf", "-inf", "101", "-1"]) +def test_gui_command_rejects_nonfinite_or_out_of_range_speed(speed): + with pytest.raises(ValueError): + command_from_fields("joints", ["0"] * 7, speed) + + +def test_gui_queue_sends_cartesian_command_data(): + queue = GuiCommandQueue(get_context("spawn").Queue()) + + queue.send_cartesian([.1, .2, .3, 0, 1, 2], 25) + + assert queue.poll() == {"type": "cartesian", "values": [.1, .2, .3, 0., 1., 2.], + "speed_percent": 25.} + queue.close() + + +def test_gui_queue_rejects_nonfinite_speed_and_maps_zero_to_stop(): + queue = GuiCommandQueue(get_context("spawn").Queue()) + + with pytest.raises(ValueError): + queue.send_joint([0] * 7, float("nan")) + queue.send_cartesian([0] * 6, 0) + assert queue.poll() == {"type": "stop"} + queue.close() + + +def test_control_gui_can_close_before_start_and_reports_closed_status(): + gui = ControlGui() + + gui.close() + + assert gui.status == "closed" + + +class FakeProcess: + def __init__(self, alive): + self.alive = iter(alive) + self.joins = [] + self.terminates = 0 + + def is_alive(self): + return next(self.alive) + + def join(self, timeout): + self.joins.append(timeout) + + def terminate(self): + self.terminates += 1 + + +def test_started_child_observer_destroys_root_after_parent_close(): + class Root: + def __init__(self): + self.destroyed = False + self.scheduled = [] + + def destroy(self): + self.destroyed = True + + def after(self, delay, callback): + self.scheduled.append((delay, callback)) + + root = Root() + closed = get_context("spawn").Event() + + _observe_parent_close(root, closed) + assert root.destroyed is False + assert len(root.scheduled) == 1 + + closed.set() + root.scheduled[0][1]() + + assert root.destroyed is True + + +def test_gui_close_joins_started_child_without_terminating_when_it_exits(): + gui = ControlGui() + gui.process = FakeProcess([True, False]) + gui._started = True + + gui.close() + + assert gui.process.joins == [2] + assert gui.process.terminates == 0 + + +def test_gui_close_terminates_started_child_only_as_fallback(): + gui = ControlGui() + gui.process = FakeProcess([True, True, False]) + gui._started = True + + gui.close() + + assert gui.process.joins == [2, 2] + assert gui.process.terminates == 1