Добавлено управление роботом

.gitignore

@@ -5,4 +5,7 @@ outputs/
 .cache/
 .codex/
 .agents/
-iiwa_description
+iiwa_description/
+docs/
+
+AGENTS.md

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`.
 

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

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")
+    preview = None
+    control_window = None
+    try:
         from .preview import Preview
         preview = Preview() if viewer and cfg["output"]["preview"] else None
-    try:
         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,8 +182,14 @@ def main():
     except KeyboardInterrupt:
         pass
     finally:
-        if preview:
+        try:
-            preview.close()
+            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()
 
 

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 <values>; "
+        "bare values also work after mode; "
+        "or use joints <A1..A7> / cartesian <x y z roll pitch yaw>; "
+        "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__}")

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

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

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