grabardm/iiwa-genesis-word
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Даниил Грабарь
2026-09-14 14:36:46 +10:00
parent ae30c02ddd
commit f7dff2e484
8 changed files with 1589 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+4 -1
View File
@@ -5,4 +5,7 @@ outputs/
.cache/
.codex/
.agents/
iiwa_description
iiwa_description/
docs/
AGENTS.md
README.md
+45 -4
View File
@@ -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 -1
View File
@@ -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
+118 -3
View File
@@ -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:
preview.close()
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()
src/iiwa_genesis/control.py
+286
View File
@@ -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
+183
View File
@@ -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
+150 -3
View File
@@ -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
+800
View File
@@ -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