Добавлен stop в mcp

Preserve dev TCP gizmo motion-priority handling while integrating Ruckig smoothing, continuous JTC spline validation and shared robot limits. Resolve launch and dependency conflicts without removing either feature set. Verified merged build of 8 packages and tests of 4 packages.

.gitignore

@@ -4,13 +4,15 @@ doc/lwc-doc/site/
 log/
 /install/
 /build/
-.vscode
-.idea
-venv
-.venv
-.claude
-.codex
-__pycache__
+.vscode/
+.idea/
+venv/
+.venv/
+.claude/
+.agents/
+.codex/
+__pycache__/
+.pytest_cache/
 
 *.egg-info
 **.FCBak

cobot-setting.yaml

@@ -3,11 +3,18 @@ robot:
   ip: "192.168.21.147"
   port: 30200
   fri_cycle_ms: 10          # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
-  joint_position_tau: 0.04  # EMA позиций FRI [с]; 0 отключает сглаживание
+  joint_position_tau: 0.06  # EMA позиций FRI [с]; 0 отключает сглаживание
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 
 
 digital_twin:
+  tcp_gizmo:
+    enabled: true              # TCP-гизма в Webots; только в режиме симуляции
+    publish_period: 0.032      # период управления [с]
+    command_timeout: 0.3       # максимальный возраст цели при отпускании мыши [с]
+    state_timeout: 0.5         # допустимый возраст состояния робота [с]
+    max_linear_speed: 0.50     # верхний предел скорости TCP [м/с]
+    max_angular_speed: 0.4     # скорость вращения TCP [рад/с]
   webots:
     world: pkg://iiwa_description/worlds/iiwa.wbt
     # world: pkg://iiwa_description/worlds/simple_world.wbt
@@ -15,6 +22,26 @@ digital_twin:
     rotation: "0 0 1 0"
     controller_timer: "50"
 
+    objects:
+      enabled: true
+      count: 1                    # точное общее количество; модели могут повторяться
+      models:
+        # - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+        #   rotation_deg: [0, 90, 0]  # X, Y, Z в градусах, относительно робота
+        # - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+        #   rotation_deg: [90, 0, 0]
+        - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+          rotation_deg: [90, 0, 0]
+        # - path: pkg://iiwa_description/objects/hammer/hammer4.obj
+        #   rotation_deg: [1.57, 0, 0]
+        # - path: pkg://iiwa_description/objects/hammer/hammer5.obj
+        #   rotation_deg: [1.57, 0, 0]
+      area:                       # метры в системе основания робота, до падения
+        x: [0.3, 0.7]
+        y: [-0.3, 0.3]
+        z: [0.1, 0.1]             # высота нижней точки модели
+
+
     cameras:
     - pkg://iiwa_config/config/cameras/d455_top.yaml
 
@@ -43,6 +70,8 @@ planning:
   default_frame: "base_link"  # Система отсчёта по умолчанию
   default_planner: "ompl"     # Планировщик по умолчанию
   planning_attempts: 3        # Число попыток планирования
+  stopped_velocity_tolerance: 0.01  # рад/с: проверка начала движения из покоя
+  state_timeout: 0.5  # с: максимальный возраст joint_states для проверки покоя
 
 web:
   enabled: true
@@ -51,6 +80,11 @@ web:
   endpoints: pkg://iiwa_config/config/api_endpoints.yaml
   joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
 
+  # Direct CSV/JSON trajectories require fresh feedback and a stationary start.
+  trajectory_state_timeout: 0.5       # s
+  trajectory_start_tolerance: 0.001   # rad
+  trajectory_stopped_velocity: 0.01  # rad/s
+
 foxglove:
   enabled: true  # Запускать ли foxglove_bridge вместе с роботом
   port: 8765 # WebSocket-порт, к которому подключается Foxglove Studio (по умолчанию 8765)

doc/lwc-doc/docs/getting-started/concepts/simulation.en.md

@@ -1,8 +1,5 @@
 # Simulation (Webots)
 
-!!! info "Work in progress"
-    A detailed simulator guide is being prepared.
-
 **Webots** is an open-source robot simulator. LWC uses it as a digital twin of the KUKA LBR IIWA 7, allowing control algorithms to be developed and debugged without a physical robot.
 
 ## Key features
@@ -11,8 +8,127 @@
 - The simulation world is set in `cobot-setting.yaml` → `digital_twin.webots.world`.
 - Start it with `cobot run --simulate`.
 
+## TCP gizmo
+
+This simulation-only mode lets you specify the robot's target pose by dragging a marker at its tool center point (TCP) in the Webots window.
+
+1. Enable `digital_twin.tcp_gizmo.enabled: true` in `cobot-setting.yaml` and launch with `cobot run --simulate`.
+2. Wait until other robot motions finish. For an initial test, use the regular named-pose command to move to `work`.
+3. Select the TCP marker in Webots. Drag a translation arrow or rotation handle with the left mouse button. The robot remains stationary while you edit the target.
+4. Release the button to submit the target. One complete trajectory is sent to the controller, rather than a stream of actions during dragging.
+5. Wait for completion before dragging again. If the target cannot be reached or the checked path collides, the marker returns to the actual TCP.
+
+Regular coordinate, joint, and control-file commands remain available. They have priority over new gizmo gestures. An already started gizmo trajectory finishes before a waiting regular command proceeds, even when that wait exceeds two seconds.
+
+### Tool attachment and speed
+
+The marker follows the active tool's `tcp` frame. With `patron`, it is at the chuck's working end; with no tool, it is at the flange. Apply tool changes through `cobot robot-setup`, rebuild, and restart the simulation. New tool models must define `tcp` at their working point.
+
+`max_linear_speed` (m/s) and `max_angular_speed` (rad/s) are upper bounds. Short moves include acceleration and braking, and joint limits may make motion slower. Joint interpolation reaches the requested TCP pose but does not guarantee a straight Cartesian path.
+
+Set `digital_twin.tcp_gizmo.enabled: false` and restart to disable this mode. The [configuration reference](../configuration.md) describes all parameters. `command_timeout` checks the age of the submitted target; mouse release is detected directly and requires no waiting-time setting.
+
+### When the marker returns or does not respond
+
+- Wait for regular motions or a committed gizmo trajectory to finish before starting another drag.
+- For a rejected target, read `TCP gizmo rejected target: ...` in the launch output. Try a smaller displacement from a reachable posture such as `work`.
+- With missing or stale robot feedback, the gizmo cannot submit movement. Check that simulation is running and `/joint_states` updates.
+
 ## Differences from the real robot
 
-- There are no real safety constraints, so motion can be faster.
+- Motion still obeys configured joint and TCP limits; simulation does not reproduce all physical robot protections.
 - Physics is approximate, including inertia, friction, and elasticity.
 - FRI is not used; communication goes through the Webots ROS 2 driver.
+
+## Spawning textured objects
+
+Set `digital_twin.webots.objects` in `cobot-setting.yaml`:
+
+```yaml
+objects:
+  enabled: true
+  count: 5
+  models:
+    - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+      rotation_deg: [0, 0, 0]
+    - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+      rotation_deg: [0, 0, 0]
+  area:
+    x: [0.3, 0.7]
+    y: [-0.3, 0.3]
+    z: [0.1, 0.1]
+```
+
+Webots spawns exactly `count` instances once at startup, independently choosing
+from `models` with replacement. This list specifies allowed models, not individual
+quantities. An absent section, `enabled: false`, or `count: 0` disables spawning.
+Paths support `pkg://`, absolute paths and paths relative to the settings file.
+Currently only Wavefront OBJ is supported.
+
+Bounds are in metres in the robot base frame. The spawner uses the same
+`webots.transform` and `webots.rotation` (axis and angle in radians) as the robot:
+`p_world = t_robot + R_robot * p_local`. They constrain initial spawn positions;
+physics may move the objects afterwards. Moving the robot with the mouse after
+startup does not relocate existing objects.
+
+The spawn anchor is the bounding-box XY centre and bottom Z. Exported vertex
+offsets are compensated without modifying the original files, UVs, scale
+(1 unit = 1 metre); orientation is configured per model. Bounds constrain the anchor, not the entire
+object. Random placements may overlap; choose a sufficiently large area.
+
+`CadShape` loads OBJ materials through `mtllib` and textures through MTL. Preserve
+relative asset paths: for example, `hammer1.mtl` references
+`hammer1_basecolor.png`. The complete `objects` directory is installed with
+`iiwa_description`. Missing models, materials or textures fail before the first
+spawn request. Physics uses an approximate bounding Box and a mass of 0.5 kg,
+not calibrated hammer inertia or grasp geometry. Objects are not automatically
+added to the MoveIt Planning Scene.
+
+The `object_spawner` node calls `/Ros2Supervisor/spawn_node_from_string`
+(`webots_ros2_msgs/srv/SpawnNodeFromString`) once per object. A rejected request
+aborts remaining work. Timeouts are not retried to avoid duplicates. The node
+exits after success and only runs in simulation.
+
+### Simulation smoke test
+
+1. Build with `colcon build --base-paths src --packages-select iiwa_description iiwa_utils iiwa_bringup iiwa_config --symlink-install`.
+2. Run `source install/setup.bash`, then `cobot run local`, selecting Webots.
+3. Inspect the five nodes `spawned_object_0000` through `spawned_object_0004`, their
+   textures and the `All 5 objects spawned` log.
+4. Change `transform` and `rotation`, restart and confirm the spawn area translates
+   and rotates with the robot base. Pause before objects fall to inspect initial positions.
+5. Check `count: 0`, operation without cameras and rejection of missing models.
+
+Reference: [Webots CadShape](https://cyberbotics.com/doc/reference/cadshape).
+
+### Per-model rotation
+
+Each `models` entry accepts `path` and `rotation_deg: [X, Y, Z]`.
+Angles are fixed degrees: apply X, then Y, then Z about the robot base axes,
+`R_model = Rz * Ry * Rx` and `R_world = R_robot * R_model`. For example:
+
+```yaml
+models:
+  - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+    rotation_deg: [0, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+    rotation_deg: [90, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+    rotation_deg: [0, 0, 45]
+```
+
+The supplied OBJ hammers have their long axis along Z. `[0, 0, 0]` gives an
+initial vertical orientation, `[90, 0, 0]` lays the hammer sideways and
+`[0, 0, 45]` turns it around the vertical axis. Both configuration files now
+explicitly set `[0, 0, 0]` for each model; adjust each independently.
+Legacy path-only entries remain supported and imply zero rotation.
+
+Both the visible model and its collision Box rotate. Height is recalculated:
+`area.z` locates the bottom of the rotated collision bounds in the robot frame;
+X/Y locate their centre. Model rotation does not rotate the spawn area.
+Changes take effect on the next launch.
+
+This sets the initial orientation, not a constraint. Gravity and contact can
+still topple the hammer. Pause simulation at spawn to inspect it. Smoke-test
+two models with different angles, check the Box stays above `area.z`, and check
+that changing `webots.rotation` preserves each model's relative orientation.

doc/lwc-doc/docs/getting-started/concepts/simulation.md

@@ -1,8 +1,5 @@
 # Симуляция (Webots)
 
-!!! info "Раздел в разработке"
-    Подробное описание работы в симуляторе готовится.
-
 **Webots** — симулятор роботов с открытым исходным кодом. В LWC используется как цифровой двойник робота KUKA LBR IIWA 7: позволяет разрабатывать и отлаживать алгоритмы управления без доступа к физическому роботу.
 
 ## Ключевые особенности
@@ -11,8 +8,132 @@
 - Мир симуляции задаётся в `cobot-setting.yaml` → `digital_twin.webots.world`
 - Для запуска: `cobot run --simulate`
 
+## Перемещение за TCP
+
+В этом режиме цифрового двойника целевая поза робота задаётся перетаскиванием маркера рабочей точки инструмента (TCP) прямо в окне Webots.
+
+1. Включите `digital_twin.tcp_gizmo.enabled: true` в `cobot-setting.yaml` и запустите `cobot run --simulate`.
+2. Дождитесь завершения других движений. Для первой проверки удобно перейти в именованную позицию `work` обычной командой.
+3. Выберите маркер TCP в Webots. Удерживая левую кнопку мыши, тяните за стрелку перемещения или ручку вращения. Во время редактирования цели робот стоит.
+4. Отпустите кнопку: контроллеру отправится одна полная траектория, а не поток action-целей при перетаскивании.
+5. Дождитесь завершения движения перед следующим переносом. При недостижимой цели или столкновении на проверяемом пути маркер возвращается к фактическому TCP.
+
+Обычные команды координат, joints и файлы управления остаются доступны и имеют приоритет перед новым перетаскиванием. Уже начатое движение гизмы завершается до выполнения ожидающей обычной команды, в том числе если ожидание длится больше двух секунд.
+
+### Инструмент и скорость
+
+Маркер следует за фреймом `tcp` активного инструмента: у `patron` это рабочий торец патрона, без инструмента — фланец. При смене инструмента примените выбор через `cobot robot-setup`, пересоберите проект и перезапустите симуляцию. В моделях новых инструментов задавайте `tcp` в рабочей точке.
+
+`max_linear_speed` (м/с) и `max_angular_speed` (рад/с) задают верхние пределы. На коротких перемещениях есть разгон и торможение; ограничения суставов также могут снизить скорость. Интерполяция в суставах достигает заданной позы TCP, но не гарантирует прямолинейный декартов путь.
+
+Для отключения задайте `digital_twin.tcp_gizmo.enabled: false` и перезапустите систему. Все параметры описаны в [справочнике конфигурации](../configuration.md). `command_timeout` проверяет возраст поступившей цели: отпускание мыши определяется напрямую, настройка времени ожидания не требуется.
+
+### Если маркер возвращается или не реагирует
+
+- Дождитесь завершения обычной команды или уже начатой траектории гизмы перед новым перетаскиванием.
+- При отказе смотрите строку `TCP gizmo rejected target: ...` в выводе запуска. Попробуйте небольшое смещение из достижимой позы, например `work`.
+- При отсутствии или устаревании обратной связи перемещение недоступно. Проверьте, что симуляция работает и `/joint_states` обновляется.
+
 ## Отличия от реального робота
 
-- Нет ограничений по безопасности (можно двигаться быстрее)
+- Действуют настроенные ограничения суставов и TCP; симуляция не воспроизводит все защиты физического робота
 - Физика не идеальна — инерция, трение и упругость приближённые
 - FRI-протокол не задействован — связь идёт через Webots-драйвер ROS 2
+
+## Спавн объектов с текстурами
+
+В `cobot-setting.yaml` (секция `digital_twin.webots`) задайте:
+
+```yaml
+objects:
+  enabled: true
+  count: 5
+  models:
+    - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+      rotation_deg: [0, 0, 0]
+    - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+      rotation_deg: [0, 0, 0]
+  area:
+    x: [0.3, 0.7]
+    y: [-0.3, 0.3]
+    z: [0.1, 0.1]
+```
+
+При запуске Webots создаётся ровно `count` объектов: каждая модель независимо
+выбирается из `models`, повторения разрешены. `enabled: false`, `count: 0` или
+отсутствие секции отключает спавн. Список — разрешённые модели, а не количество
+экземпляров каждой модели. Пути могут быть `pkg://`, абсолютными или относительно
+файла настроек. Пока поддерживается Wavefront OBJ.
+
+Область задаётся в метрах в системе основания робота: применяется та же пара
+`webots.transform` / `webots.rotation` (ось и угол в радианах), что и при спавне
+робота. Формула: `p_world = t_robot + R_robot * p_local`. Это начальная область,
+а не ограничитель движения: физика затем может перемещать объекты. Перемещение
+робота мышью после запуска не переносит уже созданные предметы.
+
+Точка спавна — центр габаритов по X/Y и нижняя точка по Z. Смещение вершин
+в экспортированном OBJ компенсируется автоматически; исходные файлы, UV,
+масштаб (1 единица = 1 метр) сохраняется; ориентация задаётся отдельно для каждой модели. Диапазон ограничивает
+эту точку, а не весь габарит. Пересечения объектов при случайном размещении
+не исключаются: выбирайте достаточно просторную область.
+
+`CadShape` загружает материалы из `mtllib` и текстуры из MTL. Сохраняйте OBJ,
+MTL и PNG в исходной структуре каталогов: например, `hammer1.mtl` ссылается на
+`hammer1_basecolor.png`. Весь каталог `objects` устанавливается вместе с
+`iiwa_description`. Отсутствующие модели, MTL и текстуры вызывают ошибку до
+первого запроса спавна. Для физики используется упрощённый габаритный Box и
+масса 0.5 кг; это не точная модель инерции или захвата молотка. В MoveIt Planning
+Scene эти объекты автоматически не добавляются.
+
+Нода `object_spawner` вызывает
+`/Ros2Supervisor/spawn_node_from_string` (`webots_ros2_msgs/srv/SpawnNodeFromString`)
+однократно для каждого предмета. Отказ сервиса прерывает оставшийся спавн;
+тайм-аут не повторяется, чтобы не создавать дубликаты. Нода завершается после
+успеха. На физическом роботе она не запускается.
+
+### Проверка в симуляции
+
+1. Пересоберите пакеты:
+   `colcon build --base-paths src --packages-select iiwa_description iiwa_utils iiwa_bringup iiwa_config --symlink-install`.
+2. Выполните `source install/setup.bash`, затем `cobot run local` и выберите Webots.
+3. Проверьте пять узлов `spawned_object_0000` … `spawned_object_0004` в дереве сцены,
+   текстуры молотков и сообщение `All 5 objects spawned`.
+4. Измените `transform` и `rotation`, перезапустите: область должна переместиться
+   и повернуться вместе с основанием робота. Для проверки начальных координат
+   используйте паузу симулятора до падения объектов.
+5. Проверьте `count: 0`, запуск без камер и ошибку при несуществующей модели.
+
+Справочник: [Webots CadShape](https://cyberbotics.com/doc/reference/cadshape).
+
+### Индивидуальный поворот модели
+
+Каждая запись `models` может содержать `path` и `rotation_deg: [X, Y, Z]`.
+Углы фиксированные, в градусах; сначала применяется поворот вокруг X, затем Y,
+затем Z в системе основания робота: `R_model = Rz * Ry * Rx`,
+`R_world = R_robot * R_model`. Например:
+
+```yaml
+models:
+  - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+    rotation_deg: [0, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+    rotation_deg: [90, 0, 0]
+  - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+    rotation_deg: [0, 0, 45]
+```
+
+В предоставленных OBJ длинная ось молотка направлена по Z: `[0, 0, 0]`
+задаёт вертикальную начальную ориентацию, `[90, 0, 0]` кладёт молоток набок,
+а `[0, 0, 45]` поворачивает его вокруг вертикальной оси. В обоих конфигурационных
+файлах для всех моделей явно указано `[0, 0, 0]`; изменяйте каждую независимо.
+Записи только с путём остаются совместимыми и означают нулевой поворот.
+
+Поворачиваются и видимая модель, и её коллизионный Box. После поворота
+пересчитывается высота: `area.z` задаёт нижнюю точку повёрнутого коллизионного
+габарита в системе робота, а X/Y — его центр. Поворот модели не поворачивает
+саму область спавна. Изменения применяются при следующем запуске.
+
+Это начальная ориентация, а не фиксация: после появления гравитация и контакты
+могут опрокинуть молоток. Для проверки поставьте симуляцию на паузу при спавне.
+Проверьте разные углы у двух моделей, отсутствие погружения Box ниже `area.z`
+и сохранение локальных углов при изменении `webots.rotation`.

doc/lwc-doc/docs/getting-started/configuration.en.md

@@ -48,6 +48,13 @@ Configures the Webots environment and RViz visualization.
 
 ```yaml
 digital_twin:
+  tcp_gizmo:
+    enabled: true
+    publish_period: 0.032
+    command_timeout: 0.3
+    state_timeout: 0.5
+    max_linear_speed: 0.50
+    max_angular_speed: 0.4
   webots:
     world: pkg://iiwa_description/worlds/iiwa.wbt
     transform: "-0.25 0 0.79"
@@ -67,6 +74,72 @@ digital_twin:
 | `webots.cameras` | List of YAML configurations for connected cameras |
 | `rviz.config` | Path to the RViz configuration |
 
+### D455 cameras in Webots
+
+The overhead camera (`config/cameras/d455_top.yaml`) and the `patron` tool camera
+use **640×480 at 30 FPS** for RGB and depth. Nominal VGA depth horizontal FOV
+is 75°; 87° applies to HD. RGB approximates a central 4:3 crop of a 1280×800
+sensor with a 90° horizontal FOV, resulting in about 79.61°. Use the individual
+camera's 640×480 calibration for an exact RGB FOV.
+
+The selected simulation depth range is 0.4–6 m with 1 mm quantization.
+These are not accuracy guarantees or the exact VGA minimum distance of a D455.
+This ideal model omits random noise, stereo matching, the IR projector,
+distortion and the physical RGB/depth offset. Webots derives vertical FOV from
+horizontal FOV and aspect ratio; it cannot be set independently.
+Matching an individual device requires its intrinsics and extrinsics.
+
+The `iiwa_utils.webots_camera.WebotsCamera` plugin maintains an average 30 Hz
+cadence in simulation time without accumulating timestep rounding errors.
+With `basicTimeStep: 32`, frame intervals are 32 or 64 ms; uniform 33.33 ms
+intervals are impossible on this grid. Wall-clock FPS depends on simulation
+speed. The world's physics timestep does not need changing.
+
+Existing image topic names are preserved:
+
+| Camera | RGB (`bgra8`) | Depth (`32FC1`, metres) |
+|---|---|---|
+| Overhead | `/d455_top/image_raw/image_color` | `/d455_top/depth/image_raw/image` |
+| Tool | `/camera/color/image_color` | `/camera/depth/image` |
+
+Each stream also publishes `camera_info` with the virtual camera matrix and
+matching timestamp. Depth offers `point_cloud` when subscribed: XYZ in metres
+using optical axes (X right, Y down, Z forward), with NaN for missing points.
+Frame IDs remain `d455_top_link` and `camera_optical_frame`;
+the plugin neither broadcasts TF nor aligns depth to RGB.
+
+Smoke test: start `cobot run local` → **Webots simulator**, subscribe to the four
+image topics, check image dimensions, matching `CameraInfo` timestamps, and the
+average rate from frame timestamps over 10 seconds of simulation time.
+`ros2 topic hz /camera/depth/image` measures wall-clock delivery, so it can report
+less than 30 when the simulation runs slowly.
+
+Sources: [D400/D455 datasheet](https://dev.realsenseai.com/download/42003/),
+[D455 RGB modes](https://support.realsenseai.com/hc/en-us/community/posts/13718098480019-How-to-resample-D455-RGB-images-from-1280-720-to-640-480),
+[Webots RangeFinder](https://cyberbotics.com/doc/reference/rangefinder).
+
+### TCP gizmo in Webots
+
+Select the TCP marker, translate or rotate it with the native Webots gizmo, and release the left mouse button. Holding the button only changes the preview. Releasing it submits one complete trajectory. Another drag is accepted after that trajectory finishes. An unreachable target or a colliding trajectory resets the marker to the current TCP.
+
+The gizmo controls the active tool's `tcp` link independently of `planning.pose_link`. For `patron`, TCP is at the chuck's working end, 55.2 mm along local Z from the `patron` link origin. Without a tool, it coincides with the `link_ee` flange. Regular Cartesian commands also use this point when `planning.pose_link: tcp`.
+
+The gizmo has the lowest motion priority. It is unavailable during regular motion. If a coordinate, joint, or file command arrives while a committed gizmo trajectory is running, that command waits for the trajectory to finish before proceeding.
+
+| `tcp_gizmo` parameter | Description |
+|---|---|
+| `enabled` | Set to `false` to disable the gizmo at the next launch; simulation only |
+| `publish_period` | State update period, seconds |
+| `command_timeout` | Maximum age of an incoming target, seconds; not a delay after dragging |
+| `state_timeout` | Maximum age of robot feedback, seconds |
+| `max_linear_speed` | TCP linear speed cap, m/s |
+| `max_angular_speed` | TCP angular speed cap, rad/s |
+
+Mouse release is detected directly, so no idle timer is needed. Joint limits, acceleration, and deceleration can reduce speed below the configured cap. The trajectory interpolates joint positions to reach the requested TCP pose; a straight Cartesian path is not guaranteed.
+
+See the [simulation guide](concepts/simulation.md) for the operating sequence.
+
+
 
 ---
 
@@ -86,6 +159,8 @@ tool:
 
 Available tools are defined in `src/iiwa_config/config/tools.yaml`. To add a tool, describe it there and then set its name in `tool.active`.
 
+Apply tool selection through `cobot robot-setup`, which updates the tool description and SRDF. Rebuild the project and restart the simulation after changing tools.
+
 ---
 
 ## `planning` section — motion planning

doc/lwc-doc/docs/getting-started/configuration.md

@@ -50,6 +50,13 @@ robot:
 
 ```yaml
 digital_twin:
+  tcp_gizmo:
+    enabled: true
+    publish_period: 0.032
+    command_timeout: 0.3
+    state_timeout: 0.5
+    max_linear_speed: 0.50
+    max_angular_speed: 0.4
   webots:
     world: pkg://iiwa_description/worlds/iiwa.wbt
     transform: "-0.25 0 0.79"
@@ -69,6 +76,71 @@ digital_twin:
 | `webots.cameras` | Список YAML-конфигов подключённых камер |
 | `rviz.config` | Путь к конфигурации RViz |
 
+### Камеры D455 в Webots
+
+Верхняя камера (`config/cameras/d455_top.yaml`) и камера инструмента `patron`
+используют **640×480, 30 кадров/с** для RGB и глубины. Горизонтальный угол глубины
+для VGA — 75°; 87° относится к HD. RGB использует приближение центральной обрезки
+сенсора 1280×800 с горизонтальным углом 90° до формата 4:3: около 79,61°.
+Точный угол RGB следует получить из калибровки конкретной камеры для 640×480.
+
+Глубина ограничена выбранным диапазоном симуляции 0,4–6 м, с шагом 1 мм.
+Это не гарантия точности и не точное значение минимальной дистанции D455 в VGA.
+Модель идеальная: без случайного шума, стереосопоставления, ИК-проектора,
+дисторсии и реального смещения между RGB и depth. Вертикальный угол Webots
+вычисляет из горизонтального угла и соотношения сторон; он не задаётся независимо.
+Для воспроизведения конкретного экземпляра нужны его intrinsics и extrinsics.
+
+Плагин `iiwa_utils.webots_camera.WebotsCamera` поддерживает среднюю частоту
+30 Гц по времени симуляции, не накапливая округление до шага Webots.
+При `basicTimeStep: 32` интервалы составляют 32 или 64 мс; равномерные интервалы
+33,33 мс на такой сетке невозможны. Частота по настенным часам зависит от скорости
+симуляции. Физический шаг мира менять не требуется.
+
+Имена топиков изображений сохранены:
+
+| Камера | RGB (`bgra8`) | Глубина (`32FC1`, метры) |
+|---|---|---|
+| Верхняя | `/d455_top/image_raw/image_color` | `/d455_top/depth/image_raw/image` |
+| Инструмент | `/camera/color/image_color` | `/camera/depth/image` |
+
+Рядом публикуется `camera_info` с матрицей виртуальной камеры и той же временной
+меткой. Для глубины доступен `point_cloud` при наличии подписчиков: XYZ в метрах
+в оптических осях (X вправо, Y вниз, Z вперёд), отсутствующие точки — NaN.
+Сохранены идентификаторы кадров `d455_top_link` и `camera_optical_frame`;
+плагин не публикует TF и не выполняет совмещение depth с RGB.
+
+Проверка после `cobot run local` → **Webots simulator**: подписаться на четыре
+топика, проверить размеры `Image`, совпадение временных меток с `CameraInfo`
+и среднюю частоту по меткам кадров за 10 секунд модельного времени.
+`ros2 topic hz /camera/depth/image` измеряет доставку по настенным часам,
+поэтому при медленной симуляции значение может быть ниже 30.
+
+Источники: [спецификация D400/D455](https://dev.realsenseai.com/download/42003/),
+[режимы RGB D455](https://support.realsenseai.com/hc/en-us/community/posts/13718098480019-How-to-resample-D455-RGB-images-from-1280-720-to-640-480),
+[Webots RangeFinder](https://cyberbotics.com/doc/reference/rangefinder).
+
+### Перемещение за TCP в Webots
+
+Порядок действий приведён в [руководстве по симуляции](concepts/simulation.md).
+
+Выберите маркер TCP, переместите или поверните его штатной гизмой Webots и отпустите левую кнопку мыши. Пока кнопка удерживается, меняется только маркер. После отпускания отправляется одна полная траектория. Следующее перетаскивание доступно после её завершения. Если цель недостижима или траектория проходит через столкновение, маркер возвращается к текущему TCP.
+
+Гизма использует звено `tcp` активного инструмента независимо от `planning.pose_link`. Для `patron` это рабочий торец патрона (55,2 мм от начала звена `patron` вдоль его локальной оси Z); без инструмента — фланец `link_ee`. Этот TCP также используется обычными декартовыми командами, когда `planning.pose_link: tcp`.
+
+У гизмы самый низкий приоритет: во время обычного движения она недоступна. Если команда координат, joints или файла приходит во время уже начатого движения гизмы, она ждёт завершения этой траектории.
+
+| Параметр `tcp_gizmo` | Описание |
+|---|---|
+| `enabled` | `false` полностью отключает гизму при следующем запуске; доступна только в симуляции |
+| `publish_period` | Период обновления состояния, с |
+| `command_timeout` | Максимальный возраст входящей цели, с; не задержка после перетаскивания |
+| `state_timeout` | Допустимый возраст обратной связи робота, с |
+| `max_linear_speed` | Верхний предел скорости TCP, м/с |
+| `max_angular_speed` | Верхний предел угловой скорости TCP, рад/с |
+
+Отпускание определяется по состоянию кнопки мыши, поэтому таймер ожидания не нужен. Скорость может быть ниже заданного предела из-за ограничений суставов, ускорения и торможения. Траектория интерполируется в суставах и достигает заданной позы TCP; прямолинейность пути TCP не гарантируется.
+
 
 ---
 
@@ -88,6 +160,8 @@ tool:
 
 Список доступных инструментов находится в файле `src/iiwa_config/config/tools.yaml`. Для добавления нового инструмента необходимо описать его там, после чего указать имя в `tool.active`.
 
+Применяйте выбор через `cobot robot-setup`: мастер обновляет описание инструмента и SRDF. После смены инструмента пересоберите проект и перезапустите симуляцию.
+
 ---
 
 ## Блок `planning` — планирование движений

doc/lwc-doc/docs/realtime-fri.en.md

@@ -82,3 +82,23 @@ sudo gpasswd -d "$USER" realtime
 After logging in again, processes use the regular Linux scheduler.
 
 See the official [ros2_control documentation](https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html#determinism) for controller-manager priorities and limits.
+
+## Trajectory limits and smoothness
+
+`controller.moveit.joint_limits` in `cobot-setting.yaml` selects a single limits file for all seven axes, consumed by xacro, FRI, Webots, MoveIt and the web API. Hard position limits and the existing narrower protective bounds remain distinct. Command validation and planning use their intersection; the MoveIt parameter override explicitly preserves soft bounds.
+
+Position and velocity limits were checked against the [KUKA LBR iiwa 7 R800 datasheet, 0000-246-832 V1.2](https://www.kuka.com/-/media/kuka-downloads/imported/8350ff3ca11642998dbdc81dcc2ed44c/0000246832_en.pdf). Working limits were not expanded relative to the previous URDF. Upward rounding beyond published values was removed. **Acceleration and jerk limits remain project settings, not verified KUKA ratings.** Tool/payload and Sunrise settings still require independent verification.
+
+OMPL/CHOMP now run TOTG → rest-boundary normalization → Ruckig → rest-boundary normalization → `iiwa_planning/LimitSplineDynamics` → MoveIt solution validation. The initial requested state must be stationary within `planning.stopped_velocity_tolerance` (rad/s). The spline adapter bounds the complete JTC quintic polynomial in Bernstein form, including extrema between waypoints. It uniformly stretches time and scales derivatives to respect velocity, acceleration and jerk limits, rejecting position overshoot. Endpoint acceleration is normalized around Ruckig because TOTG and Ruckig duration extension can leave nonzero boundary derivatives.
+
+Pilz PTP/LIN/CIRC use the spline validator and uniform time scaling without joint-space Ruckig. This preserves the original JTC interpolation curve, not a new guarantee of ideal Cartesian interpolation. Nonstationary boundary conditions, including some blended sequences, are rejected; this implementation targets rest-to-rest motions.
+
+Position-only CSV/JSON `/trajectory/send*` requests are fitted with a smooth rest-to-rest spline and passed through the same C++ validator. Input waypoints are retained; the curve between them can differ from the former linear interpolation. This direct API does not check collisions. Responses include actual `duration` and `time_scale`; timing may increase. `validate_limits=false` cannot disable mandatory dynamics and operating-range checks.
+
+Direct commands require fresh, complete named joint position/velocity feedback and a stationary robot. The first point at zero time must match the measured position within `web.trajectory_start_tolerance`; otherwise a measured start point is prepended if the first timestamp is positive. Freshness and stationarity are checked again after preparation. `web.trajectory_state_timeout` is in seconds; `web.trajectory_stopped_velocity` is in rad/s. Concurrent direct commands are rejected while a trajectory is reserved; stopping cancels pending preparation.
+
+External clients sending directly to the JTC topic/action bypass this MoveIt/web preparation and remain responsible for their trajectories. This is not a global ROS command firewall. The FRI filter, watchdog and cycle period are unchanged. Hardware retesting is needed to assess noise and actual tracking.
+
+After rebuilding, run `colcon test --packages-select iiwa_planning iiwa_utils iiwa_web iiwa_controller --python-testing pytest`. Tests exercise seven-axis consistency, spline extrema, the installed JTC interpolator, installed TOTG/Ruckig plus pluginlib, and web admission failures without connecting to a real FRI session. Rebuild `iiwa_config`, `iiwa_description` and `iiwa_bringup` as well; running processes do not update automatically.
+
+The adapters check original `/joint_states` telemetry: absent MoveIt `RobotState` velocities are not treated as zero. `planning.state_timeout` bounds telemetry age. The subscription test uses synthetic messages in separate local ROS domain 231, without FRI.

doc/lwc-doc/docs/realtime-fri.md

@@ -82,3 +82,32 @@ sudo gpasswd -d "$USER" realtime
 После повторного входа процессы снова будут работать с обычным планировщиком Linux.
 
 Подробнее о лимитах и приоритете `controller_manager` — в [документации ros2_control](https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html#determinism).
+
+## Ограничения траекторий и плавность
+
+`controller.moveit.joint_limits` из `cobot-setting.yaml` задаёт единый файл для всех семи осей. Его читают URDF/xacro, FRI, Webots, MoveIt и web API. `min_position/max_position` описывают рабочие жёсткие пределы; `soft_min_position/soft_max_position` сохраняют более узкие защитные диапазоны. Планирование, командные интерфейсы и проверка API используют пересечение этих диапазонов. При загрузке MoveIt пересечение применяется явно: YAML иначе может переопределить мягкие пределы URDF.
+
+Диапазоны и скорости сверены с [листом KUKA LBR iiwa 7 R800, 0000-246-832 V1.2](https://www.kuka.com/-/media/kuka-downloads/imported/8350ff3ca11642998dbdc81dcc2ed44c/0000246832_en.pdf). Для осей A1…A7 паспортные диапазоны составляют ±170°, ±120°, ±170°, ±120°, ±170°, ±120°, ±175°; скорости с номинальной нагрузкой — 98, 98, 100, 130, 140, 180, 180°/с. Рабочие пределы не расширены относительно прежнего URDF: округления, превышавшие паспортные значения, уменьшены. **Ускорения и рывки в YAML остаются ограничениями проекта, а не подтверждёнными паспортными пределами KUKA.** Данные установленного инструмента, нагрузки и настройки Sunrise по этому листу не проверяются.
+
+Для OMPL/CHOMP обработка выполняется в порядке:
+
+1. TOTG назначает время движения.
+2. `iiwa_planning/SetRestBoundary` проверяет скорость исходного состояния и задаёт нулевые скорости/ускорения на концах движения из покоя.
+3. Ruckig сглаживает траекторию с ограничением рывка.
+4. Повторная установка граничных условий устраняет изменения производных, которые может внести продление длительности в Ruckig.
+5. `iiwa_planning/LimitSplineDynamics` проверяет весь квинтический сплайн JTC, включая промежутки между точками. Границы полиномов вычисляются в базисе Бернштейна, а не только по временной сетке. При превышении скорости, ускорения или рывка увеличивается время всей траектории; скорости и ускорения масштабируются согласованно. Выход позиции за рабочие пределы отклоняется.
+6. MoveIt проверяет решение на столкновения обычным адаптером `ValidateSolution`.
+
+Pilz PTP/LIN/CIRC проходят проверку сплайна и равномерное увеличение длительности без Ruckig в суставном пространстве. При этом сохраняется исходная кривая интерполяции JTC; точность геометрии LIN/CIRC относительно непрерывного идеального пути отдельно не улучшается. Движения с ненулевыми граничными скоростями/ускорениями (включая некоторые варианты blending) отклоняются: проверка предназначена для перемещений из покоя в покой. `planning.stopped_velocity_tolerance` задаёт порог определения неподвижного начального состояния в рад/с.
+
+CSV/JSON `/trajectory/send*` по-прежнему принимают позиции и время, но теперь строят гладкий сплайн с нулевыми граничными производными и проходят тот же C++-валидатор. Промежуточные точки сохраняются, однако кривая между ними может отличаться от прежней линейной интерполяции. Прямой API не выполняет проверку столкновений. Ответ возвращает фактические `duration` и `time_scale`; времена могут увеличиться. `validate_limits=false` не отключает обязательную проверку динамики и рабочего диапазона.
+
+Перед отправкой требуется свежая обратная связь с именами осей, положениями и скоростями; робот должен быть неподвижен. Точка при `t=0` должна совпадать с текущим положением в пределах `web.trajectory_start_tolerance`. Если первая точка позже нуля, текущее положение добавляется в начало. `web.trajectory_state_timeout` ограничивает возраст состояния; `web.trajectory_stopped_velocity` — начальную скорость. Состояние повторно проверяется после расчёта. Повторная прямая отправка во время активной траектории отклоняется; остановка отменяет ещё не завершённую подготовку.
+
+Внешний ROS-клиент, публикующий прямо в топик JTC или отправляющий ему action, **обходит подготовку MoveIt/web**. Он обязан самостоятельно формировать корректные траектории; это изменение не является общей защитой от произвольных ROS-команд. FRI-фильтр, watchdog и период обмена не менялись. Повторное аппаратное испытание необходимо для оценки шума и фактического слежения.
+
+### Проверка без оборудования
+
+После сборки можно выполнить `colcon test --packages-select iiwa_planning iiwa_utils iiwa_web iiwa_controller --python-testing pytest`. Тесты проверяют согласованность семи осей в реальном и симуляционном URDF, пики между точками, результат установленного интерполятора JTC, связку установленного TOTG/Ruckig с pluginlib и отклонение устаревшего состояния web. Они не подключаются к реальному FRI-сеансу. Для нового кода требуется пересборка перечисленных пакетов и `iiwa_config`, `iiwa_description`, `iiwa_bringup`; старый запущенный процесс автоматически не обновляется.
+
+Проверка покоя в адаптерах подписывается на исходный `/joint_states`: отсутствие скоростей в `RobotState` MoveIt не трактуется как нулевая скорость. `planning.state_timeout` ограничивает возраст исходной телеметрии. Тест подписки использует отдельный локальный ROS domain 231 и синтетические сообщения, без FRI.

rosdep.yaml

@@ -18,3 +18,26 @@ python3-fastmcp:
     pip:
       packages: [fastmcp]
 
+# TCP gizmo / MoveIt Servo (ROS 2 Jazzy).
+moveit_servo:
+  ubuntu: [ros-jazzy-moveit-servo]
+rclcpp_action:
+  ubuntu: [ros-jazzy-rclcpp-action]
+control_msgs:
+  ubuntu: [ros-jazzy-control-msgs]
+trajectory_msgs:
+  ubuntu: [ros-jazzy-trajectory-msgs]
+action_msgs:
+  ubuntu: [ros-jazzy-action-msgs]
+rcl_interfaces:
+  ubuntu: [ros-jazzy-rcl-interfaces]
+tf2_eigen:
+  ubuntu: [ros-jazzy-tf2-eigen]
+python3-numpy:
+  ubuntu: [python3-numpy]
+python3-scipy:
+  ubuntu: [python3-scipy]
+
+# Python binding for the shared JTC spline validator.
+pybind11-dev:
+  ubuntu: [pybind11-dev]

src/iiwa_bringup/launch/iiwa.launch.py

@@ -43,11 +43,13 @@ def _runtime_setup(context, *args, **kwargs):
     if simulate:
         xacro_args = {
             "initial_positions_file": settings.controller.moveit.initial_positions,
+            "joint_limits_file": settings.controller.moveit.joint_limits,
             "simulate": "true",
         }
     else:
         xacro_args = {
             "initial_positions_file": settings.controller.moveit.initial_positions,
+            "joint_limits_file": settings.controller.moveit.joint_limits,
             "robot_ip": settings.robot.ip,
             "fri_port": str(settings.robot.port),
             "fri_cycle_ms": str(settings.robot.fri_cycle_ms),
@@ -90,6 +92,8 @@ def _runtime_setup(context, *args, **kwargs):
                 "controller_timer": str(settings.digital_twin.webots.controller_timer),
                 "controller": str(settings.controller.controller_path),
                 "initial_positions_file": str(settings.controller.moveit.initial_positions),
+                "tcp_gizmo_enabled": str(settings.digital_twin.tcp_gizmo.enabled).lower(),
+                "joint_limits_file": str(settings.controller.moveit.joint_limits),
             }.items(),
         ))
 
@@ -101,6 +105,7 @@ def _runtime_setup(context, *args, **kwargs):
             "robot_name": settings.robot.name,
             "description": description_path,
             "initial_positions_file": settings.controller.moveit.initial_positions,
+            "joint_limits_file": settings.controller.moveit.joint_limits,
             "controller_path": settings.controller.controller_path,
             "simulate": "true",
             "transform": str(settings.digital_twin.webots.transform),
@@ -112,6 +117,7 @@ def _runtime_setup(context, *args, **kwargs):
             "robot_name": settings.robot.name,
             "description": description_path,
             "initial_positions_file": settings.controller.moveit.initial_positions,
+            "joint_limits_file": settings.controller.moveit.joint_limits,
             "controller_path": settings.controller.controller_path,
             "simulate": "false",
             "transform": str(settings.digital_twin.webots.transform),

src/iiwa_bringup/launch/supported/controllers.launch.py

@@ -32,7 +32,8 @@ def _setup_controllers(context, *args, **kwargs):
         raise ValueError("joint_position_tau must be finite and non-negative")
     update_rate = 1000 // fri_cycle_ms
 
-    xacro_args = {"initial_positions_file": initial_positions_file}
+    xacro_args = {"initial_positions_file": initial_positions_file,
+                  "joint_limits_file": LaunchConfiguration("joint_limits_file").perform(context)}
 
     if simulate:
         xacro_args["simulate"] = "true"

src/iiwa_bringup/launch/supported/moveit_nodes.py

@@ -1,5 +1,7 @@
 from launch_ros.actions import Node
 from moveit_configs_utils import MoveItConfigsBuilder
+from .tcp_gizmo_nodes import make_tcp_gizmo_nodes
+from iiwa_utils.converter import effective_joint_limits
 
 
 def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
@@ -9,7 +11,8 @@ def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
         .robot_description(
             file_path=settings.robot.description,
             mappings={
-                "initial_positions_file": settings.controller.moveit.initial_positions
+                "initial_positions_file": settings.controller.moveit.initial_positions,
+                "joint_limits_file": settings.controller.moveit.joint_limits
             },
         )
         .robot_description_semantic(file_path=settings.controller.moveit.srdf)
@@ -21,10 +24,16 @@ def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
         .to_moveit_configs()
     )
 
+    moveit_configs.joint_limits["robot_description_planning"] = effective_joint_limits(
+        moveit_configs.joint_limits["robot_description_planning"]
+    )
+
     common_params = [
         moveit_configs.to_dict(),
         {"robot_description": robot_description},
         {"use_sim_time": use_sim_time},
+        {"iiwa_dynamics.stopped_velocity_tolerance": settings.planning.stopped_velocity_tolerance,
+         "iiwa_dynamics.state_timeout": settings.planning.state_timeout},
     ]
 
     move_group = Node(
@@ -46,8 +55,9 @@ def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
                 "default_frame": settings.planning.default_frame,
                 "default_planner": settings.planning.default_planner,
                 "planning_attempts": settings.planning.planning_attempts,
+                "tcp_gizmo_enabled": use_sim_time and settings.digital_twin.tcp_gizmo.enabled,
             },
         ],
     )
 
-    return moveit_configs, [move_group, move_to_pose_server]
+    return moveit_configs, [move_group, move_to_pose_server, *make_tcp_gizmo_nodes(settings, common_params, use_sim_time)]

src/iiwa_bringup/launch/supported/optional_nodes.py

@@ -27,7 +27,10 @@ def make_web_server_node(settings, use_sim_time: bool) -> Node:
             "host": settings.web.host,
             "port": settings.web.port,
             "endpoints_path": settings.web.endpoints,
-            "joint_limits_path": settings.web.joint_limits,
+            "joint_limits_path": settings.controller.moveit.joint_limits,
+            "trajectory_state_timeout": settings.web.trajectory_state_timeout,
+            "trajectory_start_tolerance": settings.web.trajectory_start_tolerance,
+            "trajectory_stopped_velocity": settings.web.trajectory_stopped_velocity,
             "use_sim_time": use_sim_time,
         }],
     )

src/iiwa_bringup/launch/supported/simulation_nodes.py

@@ -1,4 +1,5 @@
 import json
+from dataclasses import asdict
 
 from launch_ros.actions import Node
 from webots_ros2_driver.webots_controller import WebotsController
@@ -7,12 +8,23 @@ from iiwa_utils.camera_spawner import load_camera_config, build_ros_urdf  # type
 
 
 def make_simulation_nodes(settings) -> list:
-    """Возвращает ноды камер для симуляции: [camera_spawner, *camera_controllers].
-
-    Если камеры не заданы в настройках — возвращает пустой список.
-    """
+    """Create the configured object spawner and camera nodes for Webots."""
+    nodes = []
+    webots = settings.digital_twin.webots
+    if webots.objects.enabled and webots.objects.count:
+        nodes.append(Node(
+            package="iiwa_utils",
+            executable="object_spawner",
+            name="object_spawner",
+            output="screen",
+            parameters=[{
+                "objects_config": json.dumps(asdict(webots.objects)),
+                "robot_translation": webots.transform,
+                "robot_rotation": webots.rotation,
+            }],
+        ))
     if not settings.digital_twin.webots.cameras:
-        return []
+        return nodes
 
     camera_spawner = Node(
         package="iiwa_utils",
@@ -37,4 +49,4 @@ def make_simulation_nodes(settings) -> list:
             respawn=True,
         ))
 
-    return [camera_spawner, *camera_controllers]
+    return [*nodes, camera_spawner, *camera_controllers]

src/iiwa_bringup/launch/supported/tcp_gizmo_nodes.py

@@ -0,0 +1,67 @@
+"""TCP teleoperation exists only in a simulated, explicitly enabled launch."""
+from dataclasses import asdict
+import tempfile
+import xml.etree.ElementTree as ET
+
+import yaml
+from launch_ros.actions import Node
+from webots_ros2_driver.webots_controller import WebotsController
+
+
+def make_tcp_gizmo_nodes(settings, common_params, simulate):
+    cfg = settings.digital_twin.tcp_gizmo
+    if not simulate or not cfg.enabled:
+        return []
+
+    # The robot's Webots placement maps base_link into the Webots world.
+    robot = ET.Element('robot', name='tcp_gizmo')
+    ET.SubElement(robot, 'link', name='gizmo_base')
+    plugin = ET.SubElement(ET.SubElement(robot, 'webots'), 'plugin',
+                           type='iiwa_utils.tcp_gizmo.WebotsTcpGizmo')
+    for key, value in {
+        'translation': settings.digital_twin.webots.transform,
+        'rotation': settings.digital_twin.webots.rotation,
+        'frame': 'base_link',
+        'state_timeout': cfg.state_timeout,
+    }.items():
+        ET.SubElement(plugin, key).text = str(value)
+    with tempfile.NamedTemporaryFile(mode='w', prefix='iiwa_tcp_gizmo_', suffix='.yaml', delete=False) as stream:
+        yaml.safe_dump({'/**': {'ros__parameters': {
+            'robot_description': ET.tostring(robot, encoding='unicode'),
+            'set_robot_state_publisher': False,
+            'use_sim_time': True,
+        }}}, stream)
+        driver_params = stream.name
+
+    servo = {
+        'move_group_name': settings.planning.planning_group,
+        'publish_period': cfg.publish_period,
+        'max_expected_latency': max(0.1, 3 * cfg.publish_period),
+        'command_in_type': 'speed_units',
+        'scale': {'linear': cfg.max_linear_speed, 'rotational': cfg.max_angular_speed},
+        'incoming_command_timeout': cfg.command_timeout,
+        'is_primary_planning_scene_monitor': False,
+        'monitored_planning_scene_topic': '/monitored_planning_scene',
+        'joint_topic': '/joint_states',
+        'check_collisions': True,
+        'check_octomap_collisions': False,
+        'publish_joint_positions': True,
+        'publish_joint_velocities': False,
+        'use_smoothing': True,
+        'joint_limit_margins': [0.05],
+    }
+    return [
+        Node(package='iiwa_planning', executable='tcp_gizmo', name='tcp_gizmo',
+             output='screen', parameters=[*common_params, {
+                 **asdict(cfg), 'simulation': True,
+                 # Tool xacros define tcp at the working end; without a tool
+                 # it coincides with link_ee. General pose commands may use a
+                 # different planning.pose_link, but the handle controls TCP.
+                 'pose_link': 'tcp',
+                 'planning_group': settings.planning.planning_group,
+                 'default_frame': 'base_link', 'moveit_servo': servo,
+             }]),
+        Node(package='iiwa_utils', executable='tcp_gizmo_spawner', output='screen'),
+        WebotsController(robot_name='tcp_gizmo',
+                         parameters=[{'use_sim_time': True}, driver_params]),
+    ]

src/iiwa_bringup/launch/supported/webots_spawn.launch.py

@@ -22,6 +22,7 @@ def _spawn_setup(context, *args, **kwargs):
     world = LaunchConfiguration("world").perform(context)
     controller = LaunchConfiguration("controller").perform(context)
     initial_positions_file = LaunchConfiguration("initial_positions_file").perform(context)
+    gizmo_enabled = LaunchConfiguration("tcp_gizmo_enabled", default="false").perform(context) == "true"
 
     robot_description = converter.load_robot_description(
         model_path=description,
@@ -29,6 +30,7 @@ def _spawn_setup(context, *args, **kwargs):
         xacro_args={
             "simulate": "true",
             "initial_positions_file": initial_positions_file,
+            "joint_limits_file": LaunchConfiguration("joint_limits_file").perform(context),
         },
     )
 
@@ -64,6 +66,10 @@ def _spawn_setup(context, *args, **kwargs):
 
     driver = WebotsController(
         robot_name=robot_name,
+        remappings=[
+            ('/iiwa_arm_controller/joint_trajectory', '/cobot/tcp_gizmo/controller_command'),
+            ('/iiwa_arm_controller/follow_joint_trajectory', '/cobot/tcp_gizmo/follow_joint_trajectory'),
+        ] if gizmo_enabled else [],
         parameters=[
             {"use_sim_time": True},  # dict → triggers --ros-args prefix
             params_file,             # file → --params-file (robot_description + controllers)

src/iiwa_bringup/package.xml

@@ -11,6 +11,7 @@
 
   <!-- Внутренние пакеты проекта -->
   <exec_depend>iiwa_utils</exec_depend>
+  <exec_depend>iiwa_planning</exec_depend>
   <exec_depend>iiwa_config</exec_depend>
   <exec_depend>iiwa_description</exec_depend>
   <exec_depend>foxglove_bridge</exec_depend>

src/iiwa_config/config/cameras/d455_top.yaml

@@ -17,7 +17,9 @@ rotation: "0 1 0 1.5708"
 camera:
   width: 640
   height: 480
-  fieldOfView: 1.047        # ~60° в радианах
+  # VGA RGB: приближение центрального 4:3 crop сенсора 1280x800 с HFOV 90°.
+  # 2*atan(tan(90°/2)*(4/3)/(1280/800)) = 79.61°; уточнить по калибровке.
+  fieldOfView: 1.3894765524
   near: 0.1
   far: 0.0                  # 0 = без ограничений
   exposure: 1.0
@@ -40,13 +42,13 @@ camera:
 range_finder:
   width: 640
   height: 480
-  fieldOfView: 1.047
-  minRange: 0.1
-  maxRange: 5.0
-  resolution: -1.0          # -1 = без ограничений по дискретизации
+  fieldOfView: 1.308996939  # D455 VGA depth: HFOV 75°, не HD 87°
+  minRange: 0.4            # Выбранное ограничение симуляции, не точный Min-Z VGA
+  maxRange: 6.0
+  resolution: 0.001        # Шаг глубины 1 мм, не погрешность измерения
   noise: 0.0
 
   # Параметры публикации в ROS2
   ros:
     topic: /d455_top/depth/image_raw
-    update_rate: 15
+    update_rate: 30

src/iiwa_config/config/chomp_planning.yaml

@@ -0,0 +1,19 @@
+planning_plugins:
+  - chomp_interface/CHOMPPlanner
+enable_failure_recovery: true
+# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  - default_planning_response_adapters/AddTimeOptimalParameterization
+  - iiwa_planning/SetRestBoundary
+  - default_planning_response_adapters/AddRuckigTrajectorySmoothing
+  - iiwa_planning/SetRestBoundary
+  - iiwa_planning/LimitSplineDynamics
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath
+
+ridge_factor: 0.01

src/iiwa_config/config/moveit/joint_limits.yaml

@@ -1,73 +1,84 @@
-# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
-
-# For beginners, we downscale velocity and acceleration limits.
-# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+# Single source for all seven axes: URDF, ros2_control, MoveIt and web validation.
+# KUKA iiwa 7 R800 datasheet 0000-246-832 V1.2 (2022-06-01):
+# range [deg]: 170,120,170,120,170,120,175; speed [deg/s]: 98,98,100,130,140,180,180.
+# Working hard limits never exceed either that datasheet or the previous URDF.
+# Soft bounds preserve the existing protective operating margin.
+# Acceleration and jerk below are PROJECT limits, NOT verified KUKA ratings.
 default_velocity_scaling_factor: 0.1
 default_acceleration_scaling_factor: 0.1
-
-# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
-# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
 joint_limits:
   joint1:
     has_position_limits: true
-    min_position: -2.97
-    max_position: 2.97
+    min_position: -2.9670597283903604
+    max_position: 2.9670597283903604
     has_velocity_limits: true
     max_velocity: 1.71
     has_acceleration_limits: true
     max_acceleration: 8.5521
     has_jerk_limits: true
     max_jerk: 85.0
+    soft_min_position: -2.8827
+    soft_max_position: 2.8827
   joint2:
     has_position_limits: true
-    min_position: -2.10
-    max_position: 2.10
+    min_position: -2.09
+    max_position: 2.09
     has_velocity_limits: true
     max_velocity: 1.71
     has_acceleration_limits: true
     max_acceleration: 8.5521
     has_jerk_limits: true
     max_jerk: 85.0
+    soft_min_position: -2.0027
+    soft_max_position: 2.0027
   joint3:
     has_position_limits: true
-    min_position: -2.97
-    max_position: 2.97
+    min_position: -2.9670597283903604
+    max_position: 2.9670597283903604
     has_velocity_limits: true
-    max_velocity: 1.75
+    max_velocity: 1.7453292519943295
     has_acceleration_limits: true
     max_acceleration: 8.7266
     has_jerk_limits: true
     max_jerk: 87.0
+    soft_min_position: -2.8827
+    soft_max_position: 2.8827
   joint4:
     has_position_limits: true
-    min_position: -2.10
-    max_position: 2.10
+    min_position: -2.09
+    max_position: 2.09
     has_velocity_limits: true
-    max_velocity: 2.27
+    max_velocity: 2.2689280275926285
     has_acceleration_limits: true
     max_acceleration: 11.3446
     has_jerk_limits: true
     max_jerk: 113.0
+    soft_min_position: -2.0027
+    soft_max_position: 2.0027
   joint5:
     has_position_limits: true
-    min_position: -2.97
-    max_position: 2.97
+    min_position: -2.9670597283903604
+    max_position: 2.9670597283903604
     has_velocity_limits: true
     max_velocity: 2.44
     has_acceleration_limits: true
     max_acceleration: 12.2173
     has_jerk_limits: true
     max_jerk: 122.0
+    soft_min_position: -2.8827
+    soft_max_position: 2.8827
   joint6:
     has_position_limits: true
-    min_position: -2.10
-    max_position: 2.10
+    min_position: -2.09
+    max_position: 2.09
     has_velocity_limits: true
     max_velocity: 3.14
     has_acceleration_limits: true
-    max_acceleration: 15.7080
+    max_acceleration: 15.708
     has_jerk_limits: true
     max_jerk: 157.0
+    soft_min_position: -2.0027
+    soft_max_position: 2.0027
   joint7:
     has_position_limits: true
     min_position: -3.05
@@ -75,7 +86,8 @@ joint_limits:
     has_velocity_limits: true
     max_velocity: 3.14
     has_acceleration_limits: true
-    max_acceleration: 15.7080
+    max_acceleration: 15.708
     has_jerk_limits: true
     max_jerk: 157.0
-    
+    soft_min_position: -2.9627
+    soft_max_position: 2.9627

src/iiwa_config/config/ompl_planning.yaml

@@ -0,0 +1,16 @@
+planning_plugins:
+  - ompl_interface/OMPLPlanner
+# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  - default_planning_response_adapters/AddTimeOptimalParameterization
+  - iiwa_planning/SetRestBoundary
+  - default_planning_response_adapters/AddRuckigTrajectorySmoothing
+  - iiwa_planning/SetRestBoundary
+  - iiwa_planning/LimitSplineDynamics
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath

src/iiwa_config/config/pilz_industrial_motion_planner_planning.yaml

@@ -0,0 +1,16 @@
+planning_plugins:
+  - pilz_industrial_motion_planner/CommandPlanner
+default_planner_config: PTP
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  # Preserve Pilz LIN/CIRC geometry: only uniform time scaling, no joint-space Ruckig.
+  - iiwa_planning/LimitSplineDynamics
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath
+capabilities: >-
+    pilz_industrial_motion_planner/MoveGroupSequenceAction
+    pilz_industrial_motion_planner/MoveGroupSequenceService

src/iiwa_config/config/setting.yaml

@@ -8,6 +8,13 @@ robot:
 
 
 digital_twin:
+  tcp_gizmo:
+    enabled: true             # TCP-гизма в Webots; только в режиме симуляции
+    publish_period: 0.032      # период управления [с]
+    command_timeout: 0.3       # максимальный возраст цели при отпускании мыши [с]
+    state_timeout: 0.5         # допустимый возраст состояния робота [с]
+    max_linear_speed: 0.50     # верхний предел скорости TCP [м/с]
+    max_angular_speed: 0.4     # скорость вращения TCP [рад/с]
   webots:
     world: pkg://iiwa_description/worlds/iiwa.wbt
     # world: pkg://iiwa_description/worlds/simple_world.wbt
@@ -15,6 +22,26 @@ digital_twin:
     rotation: "0 0 1 0"
     controller_timer: "50"
 
+    objects:
+      enabled: true
+      count: 5                    # точное общее количество; модели могут повторяться
+      models:
+        - path: pkg://iiwa_description/objects/hammer/hammer1.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer2.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer3.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer4.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+        - path: pkg://iiwa_description/objects/hammer/hammer5.obj
+          rotation_deg: [0, 0, 0]  # X, Y, Z в градусах, относительно робота
+      area:                       # метры в системе основания робота, до падения
+        x: [0.3, 0.7]
+        y: [-0.3, 0.3]
+        z: [0.1, 0.1]             # высота нижней точки модели
+
+
     cameras:
       - pkg://iiwa_config/config/cameras/d455_top.yaml
 
@@ -43,6 +70,8 @@ planning:
   default_frame: "base_link"  # Система отсчёта по умолчанию
   default_planner: "ompl"     # Планировщик по умолчанию
   planning_attempts: 3        # Число попыток планирования
+  stopped_velocity_tolerance: 0.01  # рад/с: проверка начала движения из покоя
+  state_timeout: 0.5  # с: максимальный возраст joint_states для проверки покоя
 
 web:
   enabled: true
@@ -51,6 +80,11 @@ web:
   endpoints: pkg://iiwa_config/config/api_endpoints.yaml
   joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
 
+  # Direct CSV/JSON trajectories require fresh feedback and a stationary start.
+  trajectory_state_timeout: 0.5       # s
+  trajectory_start_tolerance: 0.001   # rad
+  trajectory_stopped_velocity: 0.01  # rad/s
+
 foxglove:
   enabled: true  # Запускать ли foxglove_bridge вместе с роботом
   port: 8765 # WebSocket-порт, к которому подключается Foxglove Studio (по умолчанию 8765)

src/iiwa_config/config/tools.yaml

@@ -15,7 +15,7 @@ tools:
     label: "Без захвата"
     xacro: null
     tip_link: "link_ee"
-    tcp_link: "link_ee"
+    tcp_link: "tcp"
     collisions: []
 
   patron:
@@ -61,4 +61,3 @@ tools:
       - [camera_hand, link2, Never]
       - [camera_hand, link1, Never]
       - [camera_hand, base_link, Never]
-

src/iiwa_controller/src/IIWAHardwareInterface.cpp

@@ -145,6 +145,11 @@ CallbackReturn IIWAHardwareInterface::on_init(
     }
     lower_[i] = limits->second.min_position;
     upper_[i] = limits->second.max_position;
+    const auto soft = info.soft_limits.find(joint.name);
+    if (soft != info.soft_limits.end()) {
+      lower_[i] = std::max(lower_[i], soft->second.min_position);
+      upper_[i] = std::min(upper_[i], soft->second.max_position);
+    }
     max_velocity_[i] = limits->second.max_velocity;
     if (!std::isfinite(lower_[i]) || !std::isfinite(upper_[i]) || lower_[i] >= upper_[i] ||
       !std::isfinite(max_velocity_[i]) || max_velocity_[i] <= 0.0)

src/iiwa_controller/test/test_hardware.cpp

@@ -107,3 +107,14 @@ TEST(Hardware, ConfiguredRealHardwareCleansUpInDependencyOrder)
   EXPECT_EQ(hardware.on_cleanup(state), Return::SUCCESS);
   EXPECT_EQ(hardware.on_error(state), Return::SUCCESS);
 }
+
+TEST(Hardware, RejectsInconsistentSoftLimits)
+{
+  Hardware hardware;
+  auto params = parameters();
+  joint_limits::SoftJointLimits soft;
+  soft.min_position = 1.0;
+  soft.max_position = -1.0;
+  params.hardware_info.soft_limits["joint1"] = soft;
+  EXPECT_EQ(hardware.on_init(params), Return::ERROR);
+}

src/iiwa_description/CMakeLists.txt

@@ -9,7 +9,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 
 install(
-  DIRECTORY resource worlds urdf
+  DIRECTORY resource worlds urdf objects
   DESTINATION share/${PROJECT_NAME}
 )
 

src/iiwa_description/objects/hammer/hammer1.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 01_|_circular_ground_face
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png
+
+newmtl 01_|_exposed_cut_wood
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png
+
+newmtl 01_|_honey_hickory_longitudinal_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png
+
+newmtl 01_|_satin_forged_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer1_basecolor.png

src/iiwa_description/objects/hammer/hammer2.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 02_|_black_oxide_forged_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png
+
+newmtl 02_|_blue_glass_fibre_resin
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png
+
+newmtl 02_|_graphite_moulded_rubber
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png
+
+newmtl 02_|_machined_steel_face
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer2_basecolor.png

src/iiwa_description/objects/hammer/hammer3.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 03_|_ash_end_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png
+
+newmtl 03_|_blue_tempered_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png
+
+newmtl 03_|_bright_milled_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png
+
+newmtl 03_|_pale_ash_straight_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer3_basecolor.png

src/iiwa_description/objects/hammer/hammer4.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 04_|_dark_walnut_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png
+
+newmtl 04_|_polished_ball_and_face
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png
+
+newmtl 04_|_walnut_end_grain
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png
+
+newmtl 04_|_warm_forged_steel
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer4_basecolor.png

src/iiwa_description/objects/hammer/hammer5.mtl

@@ -0,0 +1,42 @@
+# Blender 5.2.1 LTS MTL File: 'five_hammers.blend'
+# www.blender.org
+
+newmtl 05_|_charcoal_anti_vibration_rubber
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png
+
+newmtl 05_|_dark_textured_forged_coating
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png
+
+newmtl 05_|_machined_impact_faces
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png
+
+newmtl 05_|_orange_glass_fibre_resin
+Ns 250.000000
+Ka 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+Ke 0.000000 0.000000 0.000000
+Ni 1.500000
+d 1.000000
+illum 2
+map_Kd hammer5_basecolor.png

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -3,6 +3,8 @@
 
     <xacro:arg name="initial_positions_file"
                default="$(find iiwa_config)/config/moveit/initial_positions.yaml"/>
+    <xacro:arg name="joint_limits_file" default="$(find iiwa_config)/config/moveit/joint_limits.yaml"/>
+    <xacro:property name="joint_limits" value="${xacro.load_yaml('$(arg joint_limits_file)')['joint_limits']}"/>
     <xacro:arg name="simulate" default="false"/>
     <xacro:arg name="robot_ip" default="192.170.10.2"/>
     <xacro:arg name="fri_port" default="30200"/>
@@ -17,6 +19,7 @@
     <xacro:include filename="$(find iiwa_description)/urdf/links.xacro"/>
     <xacro:include filename="$(find iiwa_description)/urdf/joints.xacro"/>
 
+    <xacro:property name="tool_has_camera" value="false"/>
     <xacro:include filename="$(find iiwa_description)/urdf/tools/tool_active.xacro"/>
 
     <xacro:if value="$(arg simulate)">
@@ -26,6 +29,7 @@
 
             <device reference="camera_rgb" type="Camera">
                 <ros>
+                    <enabled>false</enabled>
                     <topicName>/camera/color</topicName>
                     <updateRate>30</updateRate>
                     <alwaysOn>True</alwaysOn>
@@ -35,6 +39,7 @@
 
             <device reference="camera_depth" type="RangeFinder">
                 <ros>
+                    <enabled>false</enabled>
                     <topicName>/camera/depth</topicName>
                     <updateRate>30</updateRate>
                     <alwaysOn>True</alwaysOn>
@@ -42,6 +47,23 @@
                 </ros>
             </device>
 
+            <!-- Keep the 30 Hz deadlines independent of Webots' 32 ms steps. -->
+            <xacro:if value="${tool_has_camera}">
+                <plugin type="iiwa_utils.webots_camera.WebotsCamera">
+                    <device>camera_rgb</device>
+                    <topicName>/camera/color</topicName>
+                    <updateRate>30</updateRate>
+                    <frameName>camera_optical_frame</frameName>
+                </plugin>
+                <plugin type="iiwa_utils.webots_camera.WebotsCamera">
+                    <device>camera_depth</device>
+                    <depth>true</depth>
+                    <topicName>/camera/depth</topicName>
+                    <updateRate>30</updateRate>
+                    <frameName>camera_optical_frame</frameName>
+                </plugin>
+            </xacro:if>
+
         </webots>
 
         <ros2_control name="iiwaGazeboControl" type="system">
@@ -52,8 +74,8 @@
 
             <joint name="joint1">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint1']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint1']['soft_max_position']}</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint1']}</param>
@@ -64,8 +86,8 @@
 
             <joint name="joint2">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint2']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint2']['soft_max_position']}</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint2']}</param>
@@ -76,8 +98,8 @@
 
             <joint name="joint3">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint3']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint3']['soft_max_position']}</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint3']}</param>
@@ -88,8 +110,8 @@
 
             <joint name="joint4">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint4']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint4']['soft_max_position']}</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint4']}</param>
@@ -100,8 +122,8 @@
 
             <joint name="joint5">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint5']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint5']['soft_max_position']}</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint5']}</param>
@@ -112,8 +134,8 @@
 
             <joint name="joint6">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint6']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint6']['soft_max_position']}</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint6']}</param>
@@ -124,8 +146,8 @@
 
             <joint name="joint7">
                 <command_interface name="position">
-                    <param name="min">-3.05</param>
-                    <param name="max"> 3.05</param>
+                    <param name="min">${joint_limits['joint7']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint7']['soft_max_position']}</param>
                 </command_interface>
                 <state_interface name="position">
                     <param name="initial_value">${initial_positions['joint7']}</param>
@@ -153,8 +175,8 @@
 
             <joint name="joint1">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint1']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint1']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -166,8 +188,8 @@
 
             <joint name="joint2">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint2']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint2']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -179,8 +201,8 @@
 
             <joint name="joint3">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint3']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint3']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -192,8 +214,8 @@
 
             <joint name="joint4">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint4']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint4']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -205,8 +227,8 @@
 
             <joint name="joint5">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint5']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint5']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -218,8 +240,8 @@
 
             <joint name="joint6">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint6']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint6']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -231,8 +253,8 @@
 
             <joint name="joint7">
                 <command_interface name="position">
-                    <param name="min">-3.05</param>
-                    <param name="max"> 3.05</param>
+                    <param name="min">${joint_limits['joint7']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint7']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">

src/iiwa_description/urdf/joints.xacro

@@ -12,57 +12,57 @@
     <!-- joint1: ±170° hard, ±165° soft, k_velocity=10 -->
     <xacro:revolute_joint jname="joint1" parent="base_link" child="link1"
                         xyz="0 0 0.3375" rpy="0 0 0" axis="0 0 1"
-                        lower="-2.97" upper="2.97" effort="200"
-                        velocity="1.71" damping="10.0" friction="0.1"
-                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
+                        lower="${joint_limits['joint1']['min_position']}" upper="${joint_limits['joint1']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint1']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint1']['soft_min_position']}" soft_upper="${joint_limits['joint1']['soft_max_position']}" k_velocity="10"/>
 
 
     <!-- joint2: ±119° hard, ±114° soft -->
     <xacro:revolute_joint jname="joint2" parent="link1" child="link2"
                         xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
-                        lower="-2.09" upper="2.09" effort="200"
-                        velocity="1.71" damping="10.0" friction="0.1"
-                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
+                        lower="${joint_limits['joint2']['min_position']}" upper="${joint_limits['joint2']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint2']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint2']['soft_min_position']}" soft_upper="${joint_limits['joint2']['soft_max_position']}" k_velocity="10"/>
 
 
     <!-- joint3: ±170° hard, ±165° soft -->
     <xacro:revolute_joint jname="joint3" parent="link2" child="link3"
                         xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 1"
-                        lower="-2.97" upper="2.97" effort="200"
-                        velocity="1.75" damping="10.0" friction="0.1"
-                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
+                        lower="${joint_limits['joint3']['min_position']}" upper="${joint_limits['joint3']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint3']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint3']['soft_min_position']}" soft_upper="${joint_limits['joint3']['soft_max_position']}" k_velocity="10"/>
 
 
     <!-- joint4: ±119° hard, ±114° soft -->
     <xacro:revolute_joint jname="joint4" parent="link3" child="link4"
                         xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 -1"
-                        lower="-2.09" upper="2.09" effort="200"
-                        velocity="2.27" damping="10.0" friction="0.1"
-                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
+                        lower="${joint_limits['joint4']['min_position']}" upper="${joint_limits['joint4']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint4']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint4']['soft_min_position']}" soft_upper="${joint_limits['joint4']['soft_max_position']}" k_velocity="10"/>
 
 
     <!-- joint5: ±170° hard, ±165° soft -->
     <xacro:revolute_joint jname="joint5" parent="link4" child="link5"
                         xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 1"
-                        lower="-2.97" upper="2.97" effort="200"
-                        velocity="2.44" damping="10.0" friction="0.1"
-                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
+                        lower="${joint_limits['joint5']['min_position']}" upper="${joint_limits['joint5']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint5']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint5']['soft_min_position']}" soft_upper="${joint_limits['joint5']['soft_max_position']}" k_velocity="10"/>
 
 
     <!-- joint6: ±119° hard, ±114° soft -->
     <xacro:revolute_joint jname="joint6" parent="link5" child="link6"
                         xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
-                        lower="-2.09" upper="2.09" effort="200"
-                        velocity="3.14" damping="10.0" friction="0.1"
-                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
+                        lower="${joint_limits['joint6']['min_position']}" upper="${joint_limits['joint6']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint6']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint6']['soft_min_position']}" soft_upper="${joint_limits['joint6']['soft_max_position']}" k_velocity="10"/>
 
 
     <!-- joint7: ±174° hard, ±169° soft -->
     <xacro:revolute_joint jname="joint7" parent="link6" child="link7"
                         xyz="0 -0.126 0" rpy="1.5708 0 0" axis="0 0 1"
-                        lower="-3.05" upper="3.05" effort="200"
-                        velocity="3.14" damping="10.0" friction="0.1"
-                        soft_lower="-2.9627" soft_upper="2.9627" k_velocity="10"/>
+                        lower="${joint_limits['joint7']['min_position']}" upper="${joint_limits['joint7']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint7']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint7']['soft_min_position']}" soft_upper="${joint_limits['joint7']['soft_max_position']}" k_velocity="10"/>
 
     <joint name="tools_joint" type="fixed">
         <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>

src/iiwa_description/urdf/tools/patron.xacro

@@ -1,6 +1,8 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="tool_patron">
 
+    <xacro:property name="tool_has_camera" value="true"/>
+
     <link name="camera_holder">
         <inertial>
             <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
@@ -107,7 +109,8 @@
     </joint>
 
     <joint name="patron_tcp" type="fixed">
-        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5708"/>
+        <!-- Working end of patron.dae, 55.2 mm from the patron link origin. -->
+        <origin xyz="0.0 0.0 0.0552" rpy="0.0 0.0 -1.5708"/>
         <parent link="patron"/>
         <child link="tcp"/>
     </joint>
@@ -133,7 +136,8 @@
     <gazebo reference="camera_optical_frame">
         <sensor type="camera" name="camera_rgb">
             <camera>
-                <horizontal_fov>1.3089969</horizontal_fov>
+                <!-- VGA RGB central-crop approximation: 79.61 deg; calibrate per device. -->
+                <horizontal_fov>1.3894765524</horizontal_fov>
                 <image>
                     <width>640</width>
                     <height>480</height>
@@ -145,7 +149,7 @@
                 </clip>
                 <noise>
                     <type>gaussian</type>
-                    <stddev>0.005</stddev>
+                    <stddev>0.0</stddev>
                 </noise>
             </camera>
         </sensor>
@@ -158,20 +162,22 @@
                     <height>480</height>
                 </image>
                 <clip>
-                    <near>0.52</near>
+                    <near>0.01</near>
                     <far>6.0</far>
                 </clip>
             </camera>
             <range>
-                <min>0.52</min>
+                <!-- Conservative simulation cutoff, not calibrated VGA Min-Z. -->
+                <min>0.4</min>
                 <max>6.0</max>
                 <resolution>0.001</resolution>
             </range>
             <noise>
                 <type>gaussian</type>
-                <stddev>0.02</stddev>
+                <!-- No arbitrary Gaussian noise: this is an ideal depth model. -->
+                <stddev>0.0</stddev>
             </noise>
         </sensor>
     </gazebo> 
 
-</robot>
+</robot>

src/iiwa_msgs/CMakeLists.txt

@@ -7,6 +7,9 @@ find_package(geometry_msgs REQUIRED)
 find_package(action_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/GizmoCommand.msg"
+  "msg/GizmoState.msg"
+  "srv/MotionPriority.srv"
   "srv/MoveToPose.srv"
   "srv/MoveToNamedPose.srv"
   "action/MoveToPose.action"

src/iiwa_msgs/msg/GizmoCommand.msg

@@ -0,0 +1,3 @@
+# One committed TCP target on mouse release, with generation from GizmoState.
+geometry_msgs/PoseStamped target
+uint64 epoch

src/iiwa_msgs/msg/GizmoState.msg

@@ -0,0 +1,6 @@
+# Actual TCP pose; enabled is false while any higher-priority motion owns control.
+geometry_msgs/PoseStamped actual
+bool enabled
+# Advances on rejection, controller completion, or higher-priority ownership.
+uint64 epoch
+string reason

src/iiwa_msgs/srv/MotionPriority.srv

@@ -0,0 +1,7 @@
+# Acquire/renew or release a named high-priority lease. TTL uses wall time [s].
+string owner
+bool acquire
+float64 ttl
+---
+bool success
+string message

src/iiwa_planning/CMakeLists.txt

@@ -9,6 +9,8 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(control_msgs REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(moveit_core REQUIRED)
@@ -16,6 +18,60 @@ find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(iiwa_msgs REQUIRED)
+find_package(moveit_servo REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(action_msgs REQUIRED)
+find_package(tf2_eigen REQUIRED)
+
+add_executable(tcp_gizmo src/tcp_gizmo.cpp)
+target_compile_features(tcp_gizmo PUBLIC cxx_std_17)
+target_include_directories(tcp_gizmo PRIVATE include)
+ament_target_dependencies(tcp_gizmo rclcpp rclcpp_action control_msgs iiwa_msgs geometry_msgs trajectory_msgs
+  action_msgs moveit_servo moveit_ros_planning moveit_core tf2_eigen)
+find_package(pluginlib REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module)
+find_package(pybind11 REQUIRED)
+pybind11_add_module(_iiwa_spline_limits src/spline_limits_py.cpp)
+target_compile_features(_iiwa_spline_limits PRIVATE cxx_std_17)
+target_include_directories(_iiwa_spline_limits PRIVATE include)
+ament_get_python_install_dir(PYTHON_INSTALL_DIR)
+install(TARGETS _iiwa_spline_limits LIBRARY DESTINATION ${PYTHON_INSTALL_DIR})
+
+add_library(iiwa_spline_limits SHARED src/limit_spline_dynamics.cpp)
+target_compile_features(iiwa_spline_limits PUBLIC cxx_std_17)
+target_include_directories(iiwa_spline_limits PRIVATE include)
+ament_target_dependencies(iiwa_spline_limits rclcpp moveit_core moveit_ros_planning pluginlib sensor_msgs)
+pluginlib_export_plugin_description_file(moveit_core planning_response_plugins.xml)
+install(TARGETS iiwa_spline_limits LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+  enable_testing()
+  add_executable(test_spline_limits test/test_spline_limits.cpp)
+  target_compile_features(test_spline_limits PRIVATE cxx_std_17)
+  target_compile_options(test_spline_limits PRIVATE -UNDEBUG)
+  target_include_directories(test_spline_limits PRIVATE include)
+  add_test(NAME spline_limits COMMAND test_spline_limits)
+  find_package(joint_trajectory_controller REQUIRED)
+  add_executable(test_jtc_interpolation test/test_jtc_interpolation.cpp)
+  target_compile_features(test_jtc_interpolation PRIVATE cxx_std_17)
+  target_compile_options(test_jtc_interpolation PRIVATE -UNDEBUG)
+  target_include_directories(test_jtc_interpolation PRIVATE include)
+  ament_target_dependencies(test_jtc_interpolation joint_trajectory_controller)
+  add_test(NAME jtc_interpolation COMMAND test_jtc_interpolation)
+  add_executable(test_response_adapter test/test_response_adapter.cpp)
+  target_compile_options(test_response_adapter PRIVATE -UNDEBUG)
+  ament_target_dependencies(test_response_adapter moveit_core moveit_ros_planning pluginlib)
+  add_test(NAME response_adapter COMMAND test_response_adapter)
+  add_executable(test_start_state_monitor test/test_start_state_monitor.cpp)
+  target_compile_options(test_start_state_monitor PRIVATE -UNDEBUG)
+  target_include_directories(test_start_state_monitor PRIVATE include)
+  ament_target_dependencies(test_start_state_monitor rclcpp sensor_msgs)
+  add_test(NAME start_state_monitor COMMAND test_start_state_monitor)
+  set_tests_properties(start_state_monitor PROPERTIES ENVIRONMENT "ROS_DOMAIN_ID=231;ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST" TIMEOUT 15)
+  add_test(NAME direct_trajectory COMMAND ${Python3_EXECUTABLE} -m pytest -q ${CMAKE_CURRENT_SOURCE_DIR}/test/test_prepare_trajectory.py)
+  set_tests_properties(direct_trajectory PROPERTIES ENVIRONMENT "PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_SOURCE_DIR}:$ENV{PYTHONPATH}")
+endif()
 
 add_executable(motion_planning_cpp src/motion_planning_cpp.cpp)
 target_include_directories(motion_planning_cpp PUBLIC include)
@@ -33,6 +89,7 @@ ament_target_dependencies(motion_planning_cpp
 ament_python_install_package(${PROJECT_NAME})
 
 install(TARGETS
+  tcp_gizmo
   motion_planning_cpp
   DESTINATION lib/${PROJECT_NAME}
 )
@@ -55,4 +112,14 @@ install(FILES
 )
 
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_pytest REQUIRED)
+  ament_add_gtest(test_tcp_gizmo_policy test/test_tcp_gizmo_policy.cpp)
+  target_compile_features(test_tcp_gizmo_policy PUBLIC cxx_std_17)
+  target_include_directories(test_tcp_gizmo_policy PRIVATE include)
+  ament_add_pytest_test(test_motion_priority test/test_motion_priority.py
+    APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR})
+endif()
+
 ament_package()

src/iiwa_planning/iiwa_planning/direct_trajectory.py

@@ -0,0 +1,26 @@
+"""Prepare position-only waypoints for the JTC quintic interpolator, off the RT thread."""
+import numpy as np
+from scipy.interpolate import make_interp_spline
+from _iiwa_spline_limits import retime
+
+
+def prepare_trajectory(times, positions, limits):
+    """Preserve waypoints, fit a C2+ rest-to-rest curve, bound its complete interpolation.
+
+    `times` starts at zero. Limits contain [lower, upper, velocity, acceleration, jerk].
+    Returns (times, positions, velocities, accelerations, uniform_time_scale).
+    Timing may increase; intermediate path can differ from position-only linear interpolation.
+    No collision checking is performed by this direct-command API.
+    """
+    t, q = np.asarray(times, dtype=float), np.asarray(positions, dtype=float)
+    if t.ndim != 1 or q.ndim != 2 or len(t) < 2 or len(t) != len(q):
+        raise ValueError('At least two complete trajectory points are required')
+    if not np.isfinite(t).all() or not np.isfinite(q).all() or t[0] != 0 or np.any(np.diff(t) < 1e-6):
+        raise ValueError('Finite waypoints and strictly increasing times starting at zero are required')
+    if q.shape[1] != len(limits):
+        raise ValueError('Joint count does not match limits')
+    zero = np.zeros(q.shape[1])
+    # With two derivative constraints per endpoint, knots coincide with all input times.
+    # Thus JTC quintics reproduce these spline segments from their endpoint p/v/a.
+    spline = make_interp_spline(t, q, k=5, bc_type=([(1, zero), (2, zero)], [(1, zero), (2, zero)]))
+    return retime(t.tolist(), q.tolist(), spline(t, 1).tolist(), spline(t, 2).tolist(), limits)

src/iiwa_planning/iiwa_planning/motion_priority.py

@@ -0,0 +1,146 @@
+"""Wall-clock motion reservations and serialization, independent of ROS simulation time."""
+import threading
+import uuid
+
+
+class LeaseError(RuntimeError):
+    pass
+
+
+class MotionLease:
+    """Renew one owner until context exit; transport errors fail closed."""
+    def __init__(self, exchange=None, *, on_lost=lambda: None, ttl=5.0,
+                 renew_interval=1.0):
+        self.owner = uuid.uuid4().hex
+        self._exchange = exchange
+        self._on_lost = on_lost
+        self._ttl = ttl
+        self._interval = renew_interval
+        self._stop = threading.Event()
+        self._lost = threading.Event()
+        self._thread = None
+
+    def __enter__(self):
+        if self._exchange is not None:
+            self._exchange(self.owner, True, self._ttl)
+            self._thread = threading.Thread(target=self._renew, daemon=True)
+            self._thread.start()
+        return self
+
+    def _renew(self):
+        while not self._stop.wait(self._interval):
+            try:
+                self._exchange(self.owner, True, self._ttl)
+            except Exception:
+                self._lost.set()
+                try:
+                    self._on_lost()
+                finally:
+                    return
+
+    @property
+    def lost(self):
+        return self._lost.is_set()
+
+    def check(self):
+        if self.lost:
+            raise LeaseError('Motion priority reservation lost')
+
+    def __exit__(self, exc_type, exc, tb):
+        self._stop.set()
+        if self._thread is not None:
+            self._thread.join(timeout=3.0)
+            try:
+                self._exchange(self.owner, False, self._ttl)
+            except Exception:
+                # Expiry bounds cleanup even if the service disappeared.
+                pass
+        if exc_type is None:
+            self.check()
+        return False
+
+
+class MotionSerialiser:
+    """One PlanningComponent owner; stop invalidates both planning and queued work."""
+    def __init__(self):
+        self.lock = threading.Lock()
+        self._state_lock = threading.Lock()
+        self._generation = 0
+
+    def generation(self):
+        with self._state_lock:
+            return self._generation
+
+    def check(self, generation):
+        if generation != self.generation():
+            raise LeaseError('Motion interrupted by stop')
+
+    def stop(self, callback):
+        with self._state_lock:
+            self._generation += 1
+        callback()
+
+
+def wait_response(future, timeout=2.0, *, available=lambda: True):
+    """Wait only from worker threads; an independent executor services the future."""
+    done = threading.Event()
+    future.add_done_callback(lambda _: done.set())
+    if timeout is None:
+        # Acquisition is acknowledged only after a committed gizmo trajectory
+        # finishes. Its duration can exceed the normal service RPC timeout.
+        while not done.wait(.2):
+            if not available():
+                future.cancel()
+                raise LeaseError('Motion priority service unavailable while waiting')
+    elif not done.wait(timeout):
+        future.cancel()
+        raise LeaseError('Motion priority service response timed out')
+    result = future.result()
+    if result is None:
+        raise LeaseError('Motion priority service returned no response')
+    return result
+
+
+class MotionPriorityClient:
+    """Dedicated executor prevents lease starvation under concurrent motion requests."""
+    def __init__(self, *, context=None):
+        from rclpy.node import Node
+        from rclpy.callback_groups import ReentrantCallbackGroup
+        from rclpy.executors import MultiThreadedExecutor
+        from iiwa_msgs.srv import MotionPriority
+        self._type = MotionPriority
+        self._node = Node('motion_priority_' + uuid.uuid4().hex[:12], context=context,
+                          start_parameter_services=False, use_global_arguments=False)
+        self._client = self._node.create_client(
+            MotionPriority, '/cobot/tcp_gizmo/priority',
+            callback_group=ReentrantCallbackGroup())
+        self._executor = MultiThreadedExecutor(num_threads=2, context=context)
+        self._executor.add_node(self._node)
+        self._thread = threading.Thread(target=self._spin, daemon=True)
+        self._thread.start()
+
+    def _spin(self):
+        from rclpy.executors import ExternalShutdownException
+        try:
+            self._executor.spin()
+        except ExternalShutdownException:
+            pass
+
+    def create_stop_service(self, callback):
+        from std_srvs.srv import Trigger
+        return self._node.create_service(Trigger, "/cobot/stop", callback)
+
+    def exchange(self, owner, acquire, ttl):
+        if not self._client.wait_for_service(timeout_sec=0.5):
+            raise LeaseError('Motion priority service unavailable')
+        request = self._type.Request(owner=owner, acquire=acquire, ttl=ttl)
+        result = wait_response(
+            self._client.call_async(request), timeout=None if acquire else 2.0,
+            available=lambda: self._node.context.ok() and self._client.service_is_ready())
+        if not result.success:
+            raise LeaseError(result.message)
+
+    def close(self):
+        self._executor.shutdown(timeout_sec=2.0)
+        self._thread.join(timeout=2.0)
+        self._node.destroy_node()

src/iiwa_planning/include/iiwa_planning/spline_limits.hpp

@@ -0,0 +1,147 @@
+#pragma once
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <stdexcept>
+#include <vector>
+
+namespace iiwa_planning {
+struct Limit {
+  double lower, upper, velocity, acceleration, jerk;
+};
+struct Sample {
+  double time;
+  std::vector<double> position, velocity, acceleration;
+};
+// Normalized time u=t/T; exactly the quintic used for JTC
+// position/velocity/acceleration points.
+inline std::vector<double> quintic(double p0, double p1, double v0, double v1,
+                                   double a0, double a1, double t) {
+  const double d = p1 - p0, v = v0 * t, w = v1 * t, a = a0 * t * t,
+               b = a1 * t * t;
+  return {p0,
+          v,
+          a / 2,
+          10 * d - 6 * v - 4 * w - 1.5 * a + .5 * b,
+          -15 * d + 8 * v + 7 * w + 1.5 * a - b,
+          6 * d - 3 * v - 3 * w - .5 * a + .5 * b};
+}
+inline double choose(unsigned n, unsigned k) {
+  double x = 1;
+  for (unsigned i = 1; i <= k; ++i)
+    x = x * (n + 1 - i) / i;
+  return x;
+}
+// Convex hull bound in Bernstein basis, tightened by de Casteljau subdivision.
+// Unlike sampling, it also bounds extrema between evaluation points.
+inline std::pair<double, double> hull(const std::vector<double> &b,
+                                      unsigned depth) {
+  if (!depth)
+    return {*std::min_element(b.begin(), b.end()),
+            *std::max_element(b.begin(), b.end())};
+  auto v = b;
+  std::vector<double> left(b.size()), right(b.size());
+  left[0] = v[0];
+  right.back() = v.back();
+  for (size_t k = 1; k < b.size(); ++k) {
+    for (size_t i = 0; i < b.size() - k; ++i)
+      v[i] = (v[i] + v[i + 1]) / 2;
+    left[k] = v[0];
+    right[b.size() - 1 - k] = v[b.size() - 1 - k];
+  }
+  const auto l = hull(left, depth - 1), r = hull(right, depth - 1);
+  return {std::min(l.first, r.first), std::max(l.second, r.second)};
+}
+inline std::pair<double, double> bounds(const std::vector<double> &power) {
+  std::vector<double> b(power.size(), 0);
+  const unsigned n = power.size() - 1;
+  for (unsigned i = 0; i <= n; ++i)
+    for (unsigned k = 0; k <= i; ++k)
+      b[i] += power[k] * choose(i, k) / choose(n, k);
+  return hull(b, 5);
+}
+inline double limit_splines(std::vector<Sample> &points,
+                            const std::vector<Limit> &limits) {
+  if (points.size() < 2 || limits.empty())
+    throw std::runtime_error("Need at least two trajectory points");
+  for (const auto &l : limits) {
+    for (double x : {l.lower, l.upper, l.velocity, l.acceleration, l.jerk})
+      if (!std::isfinite(x))
+        throw std::runtime_error("Non-finite joint limit");
+    if (l.lower >= l.upper || l.velocity <= 0 || l.acceleration <= 0 ||
+        l.jerk <= 0)
+      throw std::runtime_error("Invalid joint limits");
+  }
+  for (const auto &p : points) {
+    if (!std::isfinite(p.time) || p.time < 0 || p.time > 2147483647.0)
+      throw std::runtime_error("Invalid trajectory time");
+    for (const auto *values : {&p.position, &p.velocity, &p.acceleration}) {
+      if (values->size() != limits.size())
+        throw std::runtime_error("Incomplete trajectory derivatives");
+      for (double x : *values)
+        if (!std::isfinite(x))
+          throw std::runtime_error("Non-finite trajectory");
+    }
+  }
+  if (std::abs(points.front().time) > 1e-9)
+    throw std::runtime_error("Trajectory must start at t=0");
+  for (const auto *p : {&points.front(), &points.back()}) {
+    for (double v : p->velocity)
+      if (std::abs(v) > 1e-9)
+        throw std::runtime_error(
+            "Only stationary trajectory boundaries are supported");
+    for (double a : p->acceleration)
+      if (std::abs(a) > 1e-9)
+        throw std::runtime_error(
+            "Boundary acceleration must be zero for bounded jerk");
+  }
+  double scale = 1;
+  for (size_t i = 1; i < points.size(); ++i) {
+    const auto &p = points[i - 1];
+    const auto &q = points[i];
+    const double dt = q.time - p.time;
+    if (dt < 1e-6)
+      throw std::runtime_error(
+          "Trajectory times must strictly increase (>= 1 us)");
+    for (size_t j = 0; j < limits.size(); ++j) {
+      auto c = quintic(p.position[j], q.position[j], p.velocity[j],
+                       q.velocity[j], p.acceleration[j], q.acceleration[j], dt);
+      auto position = bounds(c);
+      const auto &l = limits[j];
+      if (position.first < l.lower - 1e-10 || position.second > l.upper + 1e-10)
+        throw std::runtime_error(
+            "Interpolated position exceeds joint limit at joint index " +
+            std::to_string(j));
+      for (unsigned order = 1; order <= 3; ++order) {
+        std::vector<double> derivative(c.size() - 1);
+        for (size_t k = 1; k < c.size(); ++k)
+          derivative[k - 1] = k * c[k] / dt;
+        c = derivative;
+        const auto b = bounds(c);
+        const double peak = std::max(std::abs(b.first), std::abs(b.second));
+        const double cap =
+            order == 1 ? l.velocity : (order == 2 ? l.acceleration : l.jerk);
+        const double ratio = peak / cap;
+        if (!std::isfinite(ratio))
+          throw std::runtime_error("Unrepresentable trajectory derivatives");
+        scale = std::max(scale, std::pow(ratio, 1.0 / order));
+      }
+    }
+  }
+  if (scale > 1.0 + 1e-9) {
+    scale *= 1.000001; // Margin for conversion of timestamps to nanoseconds.
+    for (auto &p : points) {
+      p.time *= scale;
+      if (!std::isfinite(p.time) || p.time > 2147483647.0)
+        throw std::runtime_error(
+            "Trajectory duration cannot be represented by ROS Duration");
+      for (double &v : p.velocity)
+        v /= scale;
+      for (double &a : p.acceleration)
+        a /= scale * scale;
+    }
+  } else
+    scale = 1;
+  return scale;
+}
+} // namespace iiwa_planning

src/iiwa_planning/include/iiwa_planning/start_state_monitor.hpp

@@ -0,0 +1,78 @@
+#pragma once
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <vector>
+
+namespace iiwa_planning {
+// MoveIt's CurrentStateMonitor may omit velocities (copy_dynamics=false).
+// Observe the original telemetry instead of treating missing dynamics as zero.
+class StartStateMonitor {
+public:
+  void initialize(const rclcpp::Node::SharedPtr &node) {
+    node_ = node;
+    for (const auto &item : std::vector<std::pair<std::string, double>>{
+             {"iiwa_dynamics.stopped_velocity_tolerance", .01},
+             {"iiwa_dynamics.state_timeout", .5}})
+      if (!node->has_parameter(item.first))
+        node->declare_parameter(item.first, item.second);
+    tolerance_ = node->get_parameter("iiwa_dynamics.stopped_velocity_tolerance")
+                     .as_double();
+    timeout_ = node->get_parameter("iiwa_dynamics.state_timeout").as_double();
+    if (!std::isfinite(tolerance_) || tolerance_ <= 0 ||
+        !std::isfinite(timeout_) || timeout_ <= 0)
+      throw std::runtime_error("Invalid start-state monitoring parameters");
+    subscription_ = node->create_subscription<sensor_msgs::msg::JointState>(
+        "/joint_states", rclcpp::SensorDataQoS(),
+        [this](sensor_msgs::msg::JointState::ConstSharedPtr msg) {
+          std::lock_guard<std::mutex> lock(mutex_);
+          latest_ = msg;
+          received_ = std::chrono::steady_clock::now();
+        });
+  }
+  bool stationary(const std::vector<std::string> &names,
+                  const sensor_msgs::msg::JointState &requested) const {
+    // Explicit nonzero requested initial dynamics are never silently discarded.
+    if (!requested.velocity.empty() && !velocities_ok(names, requested))
+      return false;
+    auto node = node_.lock();
+    if (!node)
+      return velocities_ok(names,
+                           requested); // offline use requires explicit dynamics
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!latest_ || std::chrono::duration<double>(
+                        std::chrono::steady_clock::now() - received_)
+                            .count() > timeout_)
+      return false;
+    const double age =
+        node->now().seconds() -
+        (latest_->header.stamp.sec + latest_->header.stamp.nanosec * 1e-9);
+    return std::isfinite(age) && age >= 0 && age <= timeout_ &&
+           velocities_ok(names, *latest_);
+  }
+
+private:
+  bool velocities_ok(const std::vector<std::string> &names,
+                     const sensor_msgs::msg::JointState &msg) const {
+    for (const auto &name : names) {
+      auto found = std::find(msg.name.begin(), msg.name.end(), name);
+      if (found == msg.name.end())
+        return false;
+      const size_t i = found - msg.name.begin();
+      if (i >= msg.velocity.size() || !std::isfinite(msg.velocity[i]) ||
+          std::abs(msg.velocity[i]) > tolerance_)
+        return false;
+    }
+    return true;
+  }
+  std::weak_ptr<rclcpp::Node> node_;
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription_;
+  mutable std::mutex mutex_;
+  sensor_msgs::msg::JointState::ConstSharedPtr latest_;
+  std::chrono::steady_clock::time_point received_;
+  double tolerance_{.01}, timeout_{.5};
+};
+} // namespace iiwa_planning

src/iiwa_planning/include/iiwa_planning/tcp_gizmo_policy.hpp

@@ -0,0 +1,65 @@
+#pragma once
+#include <cmath>
+#include <cstdint>
+#include <map>
+#include <string>
+
+namespace iiwa_planning {
+inline bool requiresGizmoMotion(double translation_error, double angular_error) {
+  return translation_error >= 1e-5 || angular_error >= 1e-4;
+}
+
+// Pure arbitration state. The ROS node serializes all access. Steady time is
+// used for leases/commands; simulation time is used for trajectory deadlines.
+class GizmoPolicy {
+public:
+  std::uint64_t epoch() const { return epoch_; }
+  bool hasTarget() const { return target_; }
+  void invalidate() { target_ = false; ++epoch_; }
+  bool accept(std::uint64_t epoch, double now, double timeout) {
+    if (epoch != epoch_) return false;
+    target_ = true; target_deadline_ = now + timeout; return true;
+  }
+  bool expired(double now) const { return target_ && now >= target_deadline_; }
+  bool acquire(const std::string& owner, double ttl, double now) {
+    if (owner.empty() || !std::isfinite(ttl) || ttl <= 0. || ttl > 60.) return false;
+    const auto it = leases_.find(owner);
+    if (it == leases_.end() || it->second <= now) invalidate();
+    leases_[owner] = now + ttl; settling_ = true; return true;
+  }
+  void release(const std::string& owner) { leases_.erase(owner); }
+  void normal(double deadline) { invalidate(); normal_deadline_ = deadline; settling_ = true; }
+  void action(bool active) {
+    if (active && !action_active_) { invalidate(); settling_ = true; }
+    action_active_ = active;
+  }
+  bool allowed(double steady, double sim, bool settled) {
+    for (auto it = leases_.begin(); it != leases_.end();) {
+      if (it->second <= steady) it = leases_.erase(it); else ++it;
+    }
+    if (!leases_.empty() || action_active_ || sim < normal_deadline_) return false;
+    if (settling_ && !settled) return false;
+    settling_ = false; return true;
+  }
+private:
+  std::uint64_t epoch_{0};
+  bool target_{false}, action_active_{false}, settling_{false};
+  double target_deadline_{0.}, normal_deadline_{0.};
+  std::map<std::string, double> leases_;
+};
+// A controller action owns the low slot from submission through its terminal
+// result. Releasing the mouse or requesting high-priority motion must not
+// release this slot before the already dispatched bounded step finishes.
+class LowGoalBarrier {
+public:
+  bool idle() const { return !pending_; }
+  bool begin() {
+    if (pending_) return false;
+    pending_ = true; return true;
+  }
+  void accepted(bool value) { if (!value) terminal(); }
+  void terminal() { pending_ = false; }
+private:
+  bool pending_{false};
+};
+}  // namespace iiwa_planning

src/iiwa_planning/package.xml

@@ -11,6 +11,8 @@
   <buildtool_depend>ament_cmake_python</buildtool_depend>
 
   <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>control_msgs</depend>
   <depend>rclpy</depend>
   <depend>moveit_ros_planning_interface</depend>
   <depend>moveit_core</depend>
@@ -25,9 +27,25 @@
   <depend>tf_transformations</depend>
   <depend>python3-scipy</depend>
   <depend>iiwa_msgs</depend>
+  <depend>moveit_servo</depend>
+  <depend>trajectory_msgs</depend>
+  <depend>action_msgs</depend>
+  <depend>rcl_interfaces</depend>
+  <depend>tf2_eigen</depend>
+  <depend>pluginlib</depend>
+  <depend>sensor_msgs</depend>
+  <build_depend>pybind11-dev</build_depend>
+  <exec_depend>python3-numpy</exec_depend>
 
+  <test_depend>joint_trajectory_controller</test_depend>
+  <test_depend>python3-pytest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_lint_common</test_depend>
+  <test_depend>iiwa_utils</test_depend>
+  <test_depend>controller_manager_msgs</test_depend>
+  <test_depend>python3-numpy</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/iiwa_planning/planning_response_plugins.xml

@@ -0,0 +1,8 @@
+<library path="iiwa_spline_limits">
+  <class name="iiwa_planning/LimitSplineDynamics" type="iiwa_planning::LimitSplineDynamics" base_class_type="planning_interface::PlanningResponseAdapter">
+    <description>Bound the complete JTC quintic interpolation, stretching time without changing its joint-space curve.</description>
+  </class>
+  <class name="iiwa_planning/SetRestBoundary" type="iiwa_planning::SetRestBoundary" base_class_type="planning_interface::PlanningResponseAdapter">
+    <description>Zero stationary endpoint acceleration before Ruckig; reject moving endpoints.</description>
+  </class>
+</library>

src/iiwa_planning/scripts/motion_sequence_config.json

@@ -1,42 +1,112 @@
 {
   "home": {
     "joints": [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0],
-    "speed": 0.1
+    "speed": 0.5
   },
   "waypoints": [
-    {
-      "x": 0.6,
-      "y": 0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 2.7916,
-      "speed": 0.4,
-      "planner": "ptp"
-    },
-    {
-      "x": 0.6,
-      "y": -0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 2.7916,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "joints": [0.5, 0.3, 0.0, -1.2, 0.0, 1.4, 0.0],
-      "speed": 0.2
-    },
-    {
-      "x": 0.58,
-      "y": 0.0,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 2.8791,
-      "speed": 0.3,
-      "planner": "lin"
-    }
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"}
   ]
 }

src/iiwa_planning/scripts/motion_sequence_runner.py

@@ -28,6 +28,12 @@ import rosbag2_py
 from rosidl_runtime_py.utilities import get_message
 
 from iiwa_msgs.action import MoveToJoints, MoveToPose
+from rcl_interfaces.srv import GetParameters
+from rcl_interfaces.msg import ParameterType
+from std_srvs.srv import Trigger
+from iiwa_planning.motion_priority import (
+    MotionLease, MotionPriorityClient, LeaseError, wait_response,
+)
 
 
 def _is_joints_waypoint(wp: dict) -> bool:
@@ -70,6 +76,15 @@ class MotionSequenceRunner(Node):
         )
         self.get_logger().info(f'joints_action={joints_action}  pose_action={pose_action}')
 
+        self._parameters_client = self.create_client(
+            GetParameters, '/iiwa_motion_server/get_parameters', callback_group=self._cb_group)
+        self._stop_client = self.create_client(
+            Trigger, '/cobot/stop', callback_group=self._cb_group)
+        self._priority = None
+        self._lease_active = None
+        self._aborted = threading.Event()
+        self._active_goal = None
+        self.exit_code = 1
         self._writer: rosbag2_py.SequentialWriter | None = None
         self._registered_topics: set[str] = set()
         self._subs = []
@@ -180,6 +195,9 @@ class MotionSequenceRunner(Node):
 
         def _on_goal(future):
             gh = future.result()
+            self._active_goal = gh
+            if self._aborted.is_set() and gh.accepted:
+                gh.cancel_goal_async()
             if not gh.accepted:
                 self.get_logger().error('MoveToJoints goal rejected')
                 done.set()
@@ -187,8 +205,15 @@ class MotionSequenceRunner(Node):
             gh.get_result_async().add_done_callback(_on_result)
 
         self._joints_client.send_goal_async(goal).add_done_callback(_on_goal)
-        done.wait()
-        return result_holder[0]
+        deadline = time.monotonic() + 300.0
+        while not done.wait(0.1):
+            if self._aborted.is_set() or time.monotonic() >= deadline or not rclpy.ok():
+                self._abort_sequence()
+                return False
+            if self._lease_active:
+                self._lease_active.check()
+        self._active_goal = None
+        return result_holder[0] and not self._aborted.is_set()
 
     def _send_pose_goal(self, wp: dict, idx: int | None = None) -> bool:
         goal = MoveToPose.Goal()
@@ -215,6 +240,9 @@ class MotionSequenceRunner(Node):
 
         def _on_goal(future):
             gh = future.result()
+            self._active_goal = gh
+            if self._aborted.is_set() and gh.accepted:
+                gh.cancel_goal_async()
             if not gh.accepted:
                 self.get_logger().error('MoveToPose goal rejected')
                 done.set()
@@ -222,8 +250,15 @@ class MotionSequenceRunner(Node):
             gh.get_result_async().add_done_callback(_on_result)
 
         self._pose_client.send_goal_async(goal).add_done_callback(_on_goal)
-        done.wait()
-        return result_holder[0]
+        deadline = time.monotonic() + 300.0
+        while not done.wait(0.1):
+            if self._aborted.is_set() or time.monotonic() >= deadline or not rclpy.ok():
+                self._abort_sequence()
+                return False
+            if self._lease_active:
+                self._lease_active.check()
+        self._active_goal = None
+        return result_holder[0] and not self._aborted.is_set()
 
     def _send_waypoint(self, wp: dict, idx: int | None = None) -> bool:
         if _is_joints_waypoint(wp):
@@ -231,21 +266,49 @@ class MotionSequenceRunner(Node):
         return self._send_pose_goal(wp, idx=idx)
 
 
+    def _abort_sequence(self):
+        self._aborted.set()
+        if self._active_goal is not None and self._active_goal.accepted:
+            self._active_goal.cancel_goal_async()
+        # Asynchronous so renewal never waits on the motion server's callback pool.
+        if self._stop_client.service_is_ready():
+            self._stop_client.call_async(Trigger.Request())
+
+    def _discover_priority(self):
+        if not self._parameters_client.wait_for_service(timeout_sec=5.0):
+            raise LeaseError('Motion server parameter service unavailable')
+        response = wait_response(self._parameters_client.call_async(
+            GetParameters.Request(names=['tcp_gizmo_enabled'])))
+        if len(response.values) != 1 or response.values[0].type != ParameterType.PARAMETER_BOOL:
+            raise LeaseError('Motion server tcp_gizmo_enabled parameter unavailable')
+        if response.values[0].bool_value:
+            self._priority = MotionPriorityClient(context=self.context)
+
     def run(self, done_event: threading.Event):
+        self.exit_code = 1
         try:
-            for i in range(self._n_iter):
-                self.get_logger().info(f'======= Iteration {i + 1}/{self._n_iter} =======')
-
-                self._send_waypoint(self._home)
-
-                for idx, wp in enumerate(self._waypoints):
-                    self._send_waypoint(wp, idx=idx)
-
-                self._send_waypoint(self._home)
-                time.sleep(self._delay)
-
+            self._discover_priority()
+            with MotionLease(self._priority.exchange if self._priority else None,
+                             on_lost=self._abort_sequence) as lease:
+                self._lease_active = lease
+                for i in range(self._n_iter):
+                    self.get_logger().info(f'======= Iteration {i + 1}/{self._n_iter} =======')
+                    for idx, wp in enumerate([self._home, *self._waypoints, self._home]):
+                        lease.check()
+                        if self._aborted.is_set() or not self._send_waypoint(wp, idx=idx):
+                            raise LeaseError(f'Sequence interrupted at waypoint {idx}')
+                    if self._aborted.wait(max(0.0, self._delay)):
+                        raise LeaseError('Sequence reservation lost during pause')
+                    lease.check()
             self.get_logger().info('======= Sequence complete =======')
+            self.exit_code = 0
+        except Exception as exc:
+            self._abort_sequence()
+            self.get_logger().error(f'Sequence failed: {exc}')
         finally:
+            self._lease_active = None
+            if self._priority:
+                self._priority.close()
             self.close_bag()
             done_event.set()
 
@@ -264,12 +327,16 @@ def main():
         while not done_event.is_set():
             executor.spin_once(timeout_sec=0.1)
     except KeyboardInterrupt:
-        pass
+        node._abort_sequence()
+        deadline = time.monotonic() + 5.0
+        while not done_event.is_set() and time.monotonic() < deadline:
+            executor.spin_once(timeout_sec=0.1)
     finally:
         node.close_bag()
         node.destroy_node()
         rclpy.shutdown()
+    return node.exit_code
 
 
 if __name__ == '__main__':
-    main()
+    raise SystemExit(main())

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -17,6 +17,9 @@ from moveit.core.robot_state import RobotState
 
 from iiwa_msgs.action import MoveToPose, MoveToJoints
 from iiwa_msgs.srv import MoveToNamedPose
+from iiwa_planning.motion_priority import (
+    MotionLease, MotionPriorityClient, MotionSerialiser, LeaseError,
+)
 
 
 # Таблица планировщиков
@@ -48,6 +51,10 @@ class IiwaMotionServer(Node):
         super().__init__("iiwa_motion_server")
         self._setup_parameters()
         self._setup_moveit()
+        self._motion_serial = MotionSerialiser()
+        self._operation = threading.local()
+        self._priority = MotionPriorityClient(context=self.context)
+        self._gizmo_enabled = self.get_parameter("tcp_gizmo_enabled").value
         self._setup_servers()
 
         self.get_logger().info("Сервер движения iiwa запущен")
@@ -59,6 +66,7 @@ class IiwaMotionServer(Node):
         self.declare_parameter("default_frame", "base_link")
         self.declare_parameter("default_planner", "ompl")
         self.declare_parameter("planning_attempts", 3)
+        self.declare_parameter("tcp_gizmo_enabled", False)
 
         self._pose_link = self.get_parameter("pose_link").value
         self._planning_group = self.get_parameter("planning_group").value
@@ -67,7 +75,7 @@ class IiwaMotionServer(Node):
         self._planning_attempts = self.get_parameter("planning_attempts").value
 
     def _setup_moveit(self):
-        self._moveit = MoveItPy(node_name="iiwa_motion_server")
+        self._moveit = MoveItPy(node_name="iiwa_motion_moveit")
         self._arm: PlanningComponent = self._moveit.get_planning_component(self._planning_group)
         self._robot_model = self._moveit.get_robot_model()
 
@@ -75,19 +83,67 @@ class IiwaMotionServer(Node):
         cb = ReentrantCallbackGroup()
 
         ActionServer(
-            self, MoveToPose, "cobot/move_to_pose", self._execute_pose,
+            self, MoveToPose, "cobot/move_to_pose", lambda gh: self._guard_action(gh, MoveToPose.Result, self._execute_pose),
             callback_group=cb,
             goal_callback=lambda _: GoalResponse.ACCEPT,
             cancel_callback=lambda _: CancelResponse.ACCEPT,
         )
         ActionServer(
-            self, MoveToJoints, "cobot/move_to_joints", self._execute_joints,
+            self, MoveToJoints, "cobot/move_to_joints", lambda gh: self._guard_action(gh, MoveToJoints.Result, self._execute_joints),
             callback_group=cb,
             goal_callback=lambda _: GoalResponse.ACCEPT,
             cancel_callback=lambda _: CancelResponse.ACCEPT,
         )
-        self.create_service(MoveToNamedPose, "cobot/move_to_named", self._handle_named, callback_group=cb)
-        self.create_service(Trigger, "cobot/stop", self._handle_stop, callback_group=cb)
+        self.create_service(MoveToNamedPose, "cobot/move_to_named", self._guard_named, callback_group=cb)
+        self._priority.create_stop_service(self._handle_stop)
+
+    def _stop_execution(self):
+        self._motion_serial.stop(
+            lambda: self._moveit.get_trajectory_execution_manager().stop_execution())
+
+    def _lease(self):
+        return MotionLease(self._priority.exchange if self._gizmo_enabled else None,
+                           on_lost=self._stop_execution)
+
+    def _guarded(self, callback):
+        generation = self._motion_serial.generation()
+        # Reserve before waiting so queued requests also exclude the gizmo.
+        with self._lease() as lease:
+            with self._motion_serial.lock:
+                self._motion_serial.check(generation)
+                lease.check()
+                self._operation.generation = generation
+                self._operation.lease = lease
+                return callback()
+
+    def _guard_action(self, goal_handle, result_type, callback):
+        try:
+            result, ok, message = self._guarded(lambda: callback(goal_handle))
+            return self._finish_action(goal_handle, result, ok, message)
+        except Exception as exc:
+            result = result_type()
+            result.success = False
+            result.message = str(exc)
+            if goal_handle.is_active:
+                goal_handle.abort()
+            return result
+
+    def _guard_named(self, request, response):
+        try:
+            return self._guarded(lambda: self._handle_named(request, response))
+        except Exception as exc:
+            response.success = False
+            response.message = str(exc)
+            return response
+
+    def _check_operation(self):
+        self._operation.lease.check()
+        self._motion_serial.check(self._operation.generation)
+
+    def destroy_node(self):
+        if self._priority:
+            self._priority.close()
+        return super().destroy_node()
 
     def _make_plan_params(self, pipeline: str, planner_id: str, plan_time: float, velocity_scale: float, accel_scale: float | None = None) -> PlanRequestParameters:
         params = PlanRequestParameters(self._moveit, self._planning_group)
@@ -105,19 +161,29 @@ class IiwaMotionServer(Node):
         Возвращает (True, msg) при успехе, (False, msg) при ошибке,
         (None, 'canceled') если цель была отменена.
         """
+        self._check_operation()
         plan_result = self._arm.plan(single_plan_parameters=plan_params)
         if not plan_result:
             return False, "Планирование не удалось: поза недостижима или в столкновении"
 
+        self._check_operation()
         if goal_handle.is_cancel_requested:
             return None, "canceled"
 
         done = threading.Event()
         failed = threading.Event()
 
+        generation = self._operation.generation
+        lease = self._operation.lease
+
         def do_execute():
             try:
-                self._moveit.execute(plan_result.trajectory, controllers=[])
+                lease.check()
+                self._motion_serial.check(generation)
+                execution = self._moveit.execute(plan_result.trajectory, controllers=[])
+                # Jazzy ExecutionStatus.__bool__ is true only for SUCCEEDED.
+                if not bool(execution):
+                    failed.set()
             except Exception as exc:
                 self.get_logger().error(f"Ошибка выполнения траектории: {exc}")
                 failed.set()
@@ -126,14 +192,21 @@ class IiwaMotionServer(Node):
 
         threading.Thread(target=do_execute, daemon=True).start()
 
+        interrupted = False
         while not done.wait(timeout=0.05):
-            if goal_handle.is_cancel_requested:
+            if (goal_handle.is_cancel_requested or generation != self._motion_serial.generation()
+                    or lease.lost):
+                interrupted = True
+                # Repeat to cover stop racing the blocking execute() submission.
                 try:
                     self._moveit.get_trajectory_execution_manager().stop_execution()
-                except Exception:
-                    pass
-                done.wait()
-                return None, "canceled"
+                except Exception as exc:
+                    self.get_logger().error(f"Stop retry failed: {exc}")
+        if goal_handle.is_cancel_requested:
+            return None, "canceled"
+        self._check_operation()
+        if interrupted:
+            return False, "Выполнение траектории остановлено"
 
         if failed.is_set():
             return False, "Выполнение траектории завершилось ошибкой"
@@ -165,8 +238,7 @@ class IiwaMotionServer(Node):
             result.success = False
             result.message = f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}"
             self.get_logger().error(result.message)
-            goal_handle.abort()
-            return result
+            return result, False, result.message
 
         pipeline, planner_id, plan_time = PLANNERS[planner_key]
 
@@ -198,7 +270,7 @@ class IiwaMotionServer(Node):
 
         plan_params = self._make_plan_params(pipeline, planner_id, plan_time, velocity_scale)
         ok, msg = self._plan_and_execute(plan_params, goal_handle)
-        return self._finish_action(goal_handle, result, ok, msg)
+        return result, ok, msg
 
     def _execute_joints(self, goal_handle):
         req = goal_handle.request
@@ -230,7 +302,7 @@ class IiwaMotionServer(Node):
             "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
         )
         ok, msg = self._plan_and_execute(plan_params, goal_handle)
-        return self._finish_action(goal_handle, result, ok, msg)
+        return result, ok, msg
 
     def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):
         name = request.name.strip()
@@ -254,30 +326,26 @@ class IiwaMotionServer(Node):
         plan_params = self._make_plan_params(
             "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale, accel_scale
         )
-        plan_result = self._arm.plan(single_plan_parameters=plan_params)
-        if not plan_result:
-            response.success = False
-            response.message = f"Не удалось построить траекторию для '{name}'"
-            self.get_logger().error(response.message)
-            return response
+        class NamedGoal:
+            is_cancel_requested = False
 
-        exec_result = self._moveit.execute(plan_result.trajectory, controllers=[])
-        if exec_result:
-            response.success = True
-            response.message = f"Переместился в '{name}'"
-            self.get_logger().info(response.message)
-        else:
-            response.success = False
-            response.message = f"Выполнение траектории для '{name}' прервано (hardware fault?)"
-            self.get_logger().error(response.message)
+        ok, message = self._plan_and_execute(plan_params, NamedGoal())
+        response.success = bool(ok)
+        response.message = message
         return response
 
     def _handle_stop(self, request: Trigger.Request, response: Trigger.Response):
         try:
-            self._moveit.get_trajectory_execution_manager().stop_execution()
+            with self._lease():
+                self._stop_execution()
             response.success = True
             response.message = "Выполнение траектории остановлено"
         except Exception as exc:
+            # Failure to reserve must never prevent the requested high-priority stop.
+            try:
+                self._stop_execution()
+            except Exception as stop_exc:
+                self.get_logger().error(f"Stop failed: {stop_exc}")
             response.success = False
             response.message = str(exc)
         self.get_logger().info(f"[stop] {response.message}")
@@ -294,7 +362,10 @@ def main(args=None):
     except (KeyboardInterrupt, ExternalShutdownException):
         pass
     finally:
-        rclpy.shutdown()
+        if "node" in locals():
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
 
 
 if __name__ == "__main__":

src/iiwa_planning/src/limit_spline_dynamics.cpp

@@ -0,0 +1,140 @@
+#include "iiwa_planning/spline_limits.hpp"
+#include "iiwa_planning/start_state_monitor.hpp"
+#include <moveit/planning_interface/planning_response_adapter.hpp>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_trajectory/robot_trajectory.hpp>
+#include <pluginlib/class_list_macros.hpp>
+
+namespace iiwa_planning {
+// TOTG can supply nonzero endpoint acceleration. Establish rest boundary
+// conditions before Ruckig, which otherwise preserves them. Positions and
+// interior derivatives stay intact.
+class SetRestBoundary : public planning_interface::PlanningResponseAdapter {
+public:
+  void initialize(const rclcpp::Node::SharedPtr &node,
+                  const std::string &) override {
+    start_monitor_.initialize(node);
+  }
+  std::string getDescription() const override {
+    return "Establish intended rest boundaries around Ruckig";
+  }
+  void adapt(const planning_scene::PlanningSceneConstPtr &,
+             const planning_interface::MotionPlanRequest &req,
+             planning_interface::MotionPlanResponse &res) const override {
+    if (!res.trajectory || res.trajectory->getWayPointCount() < 2 ||
+        !res.trajectory->getGroup()) {
+      res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
+      return;
+    }
+    if (!start_monitor_.stationary(
+            res.trajectory->getGroup()->getVariableNames(),
+            req.start_state.joint_state)) {
+      RCLCPP_ERROR(
+          rclcpp::get_logger("iiwa_spline_limits"),
+          "Fresh, stationary joint_states required for rest-to-rest planning");
+      res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE;
+      return;
+    }
+    for (const auto index :
+         {size_t(0), res.trajectory->getWayPointCount() - 1}) {
+      auto &state = *res.trajectory->getWayPointPtr(index);
+      for (const auto &name : res.trajectory->getGroup()->getVariableNames()) {
+        state.setVariableVelocity(name, 0.0);
+        state.setVariableAcceleration(name, 0.0);
+      }
+    }
+  }
+
+private:
+  StartStateMonitor start_monitor_;
+};
+class LimitSplineDynamics : public planning_interface::PlanningResponseAdapter {
+public:
+  void initialize(const rclcpp::Node::SharedPtr &node,
+                  const std::string &) override {
+    start_monitor_.initialize(node);
+  }
+  std::string getDescription() const override {
+    return "Limit JTC quintic velocity, acceleration and jerk";
+  }
+  void adapt(const planning_scene::PlanningSceneConstPtr &,
+             const planning_interface::MotionPlanRequest &req,
+             planning_interface::MotionPlanResponse &res) const override {
+    if (!res.trajectory || res.trajectory->getWayPointCount() < 2) {
+      res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
+      return;
+    }
+    try {
+      auto &trajectory = *res.trajectory;
+      const auto *group = trajectory.getGroup();
+      if (!group)
+        throw std::runtime_error("Missing trajectory group");
+      const auto &names = group->getVariableNames();
+      if (!start_monitor_.stationary(names, req.start_state.joint_state))
+        throw std::runtime_error(
+            "Fresh stationary joint_states required before spline validation");
+      const auto model = trajectory.getRobotModel();
+      auto scaling = [](double x) {
+        return std::isfinite(x) && x > 0 && x <= 1 ? x : 1.0;
+      };
+      std::vector<Limit> limits;
+      for (const auto &name : names) {
+        const auto &b = model->getVariableBounds(name);
+        if (!b.position_bounded_ || !b.velocity_bounded_ ||
+            !b.acceleration_bounded_ || !b.jerk_bounded_)
+          throw std::runtime_error(
+              "Missing position/velocity/acceleration/jerk bounds: " + name);
+        limits.push_back(
+            {b.min_position_, b.max_position_,
+             b.max_velocity_ * scaling(req.max_velocity_scaling_factor),
+             b.max_acceleration_ * scaling(req.max_acceleration_scaling_factor),
+             b.max_jerk_});
+      }
+      std::vector<Sample> samples;
+      double t = 0;
+      for (size_t i = 0; i < trajectory.getWayPointCount(); ++i) {
+        const auto &state = *trajectory.getWayPointPtr(i);
+        if (!state.hasVelocities() || !state.hasAccelerations())
+          throw std::runtime_error(
+              "JTC guard requires positions, velocities and accelerations");
+        t += trajectory.getWayPointDurationFromPrevious(i);
+        Sample s{t, {}, {}, {}};
+        for (const auto &name : names) {
+          s.position.push_back(state.getVariablePosition(name));
+          s.velocity.push_back(state.getVariableVelocity(name));
+          s.acceleration.push_back(state.getVariableAcceleration(name));
+        }
+        samples.push_back(s);
+      }
+      const double scale = limit_splines(samples, limits);
+      if (scale > 1) {
+        for (size_t i = 0; i < samples.size(); ++i) {
+          auto &state = *trajectory.getWayPointPtr(i);
+          for (size_t j = 0; j < names.size(); ++j) {
+            state.setVariableVelocity(names[j], samples[i].velocity[j]);
+            state.setVariableAcceleration(names[j], samples[i].acceleration[j]);
+          }
+          trajectory.setWayPointDurationFromPrevious(
+              i, trajectory.getWayPointDurationFromPrevious(i) * scale);
+        }
+        RCLCPP_INFO(
+            rclcpp::get_logger("iiwa_spline_limits"),
+            "Trajectory duration scaled by %.6f to bound JTC derivatives",
+            scale);
+      }
+    } catch (const std::exception &e) {
+      RCLCPP_ERROR(rclcpp::get_logger("iiwa_spline_limits"),
+                   "Rejected trajectory: %s", e.what());
+      res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
+    }
+  }
+
+private:
+  StartStateMonitor start_monitor_;
+};
+} // namespace iiwa_planning
+PLUGINLIB_EXPORT_CLASS(iiwa_planning::LimitSplineDynamics,
+                       planning_interface::PlanningResponseAdapter)
+
+PLUGINLIB_EXPORT_CLASS(iiwa_planning::SetRestBoundary,
+                       planning_interface::PlanningResponseAdapter)

src/iiwa_planning/src/spline_limits_py.cpp

@@ -0,0 +1,30 @@
+#include "iiwa_planning/spline_limits.hpp"
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+namespace py = pybind11;
+PYBIND11_MODULE(_iiwa_spline_limits, m) {
+  m.def("retime", [](const std::vector<double> &t,
+                     const std::vector<std::vector<double>> &q,
+                     const std::vector<std::vector<double>> &v,
+                     const std::vector<std::vector<double>> &a,
+                     const std::vector<std::array<double, 5>> &caps) {
+    if (q.size() != t.size() || v.size() != t.size() || a.size() != t.size())
+      throw std::runtime_error("Point array sizes differ");
+    std::vector<iiwa_planning::Sample> points;
+    for (size_t i = 0; i < t.size(); ++i)
+      points.push_back({t[i], q[i], v[i], a[i]});
+    std::vector<iiwa_planning::Limit> limits;
+    for (const auto &c : caps)
+      limits.push_back({c[0], c[1], c[2], c[3], c[4]});
+    const double scale = iiwa_planning::limit_splines(points, limits);
+    std::vector<double> times;
+    std::vector<std::vector<double>> positions, velocities, accelerations;
+    for (const auto &p : points) {
+      times.push_back(p.time);
+      positions.push_back(p.position);
+      velocities.push_back(p.velocity);
+      accelerations.push_back(p.acceleration);
+    }
+    return py::make_tuple(times, positions, velocities, accelerations, scale);
+  });
+}

src/iiwa_planning/src/tcp_gizmo.cpp

@@ -0,0 +1,471 @@
+#include <iiwa_planning/tcp_gizmo_policy.hpp>
+#include <iiwa_msgs/msg/gizmo_command.hpp>
+#include <iiwa_msgs/msg/gizmo_state.hpp>
+#include <iiwa_msgs/srv/motion_priority.hpp>
+#include <control_msgs/action/follow_joint_trajectory.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <moveit_servo/servo.hpp>
+#include <moveit_servo/utils/common.hpp>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
+#include <Eigen/Geometry>
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <memory>
+#include <deque>
+#include <functional>
+#include <map>
+#include <stdexcept>
+
+namespace iiwa_planning {
+class TcpGizmo {
+  using Trajectory = trajectory_msgs::msg::JointTrajectory;
+  using Priority = iiwa_msgs::srv::MotionPriority;
+  using Follow = control_msgs::action::FollowJointTrajectory;
+  using ClientGoal = rclcpp_action::ClientGoalHandle<Follow>;
+  using ServerGoal = rclcpp_action::ServerGoalHandle<Follow>;
+  struct ProxyGoal {
+    std::shared_ptr<ServerGoal> upstream;
+    ClientGoal::SharedPtr downstream;
+    bool cancel_requested{false};
+    bool cancel_sent{false};
+    bool finished{false};
+  };
+public:
+  explicit TcpGizmo(const rclcpp::Node::SharedPtr& node) : node_(node) {
+    if (!param<bool>("enabled", false) || !param<bool>("simulation", false))
+      throw std::runtime_error("TCP gizmo requires enabled=true and simulation=true");
+    group_name_ = param<std::string>("planning_group", "iiwa_arm");
+    tcp_ = param<std::string>("pose_link", "tcp");
+    frame_ = param<std::string>("default_frame", "base_link");
+    period_ = positive("publish_period", .032);
+    command_timeout_ = positive("command_timeout", .3);
+    state_timeout_ = positive("state_timeout", .5);
+    linear_speed_ = positive("max_linear_speed", .10);
+    angular_speed_ = positive("max_angular_speed", .4);
+    listener_ = std::make_shared<servo::ParamListener>(node_, "moveit_servo");
+    const auto params = listener_->get_params();
+    if (params.move_group_name != group_name_ || std::abs(params.publish_period - period_) > 1e-9)
+      throw std::runtime_error("Servo group/period must match TCP gizmo parameters");
+    scene_ = moveit_servo::createPlanningSceneMonitor(node_, params);
+    if (!scene_ || !scene_->getRobotModel()) throw std::runtime_error("Missing planning scene");
+    group_ = scene_->getRobotModel()->getJointModelGroup(group_name_);
+    if (!group_ || !group_->getSolverInstance()) throw std::runtime_error("Missing IK solver");
+    tip_ = group_->getSolverInstance()->getTipFrame();
+    const auto model = scene_->getRobotModel();
+    if (!model->hasLinkModel(tcp_) || !model->hasLinkModel(frame_) || !model->hasLinkModel(tip_))
+      throw std::runtime_error("Unknown TCP, frame or solver tip");
+    if (moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(model->getLinkModel(tcp_)) !=
+        moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(model->getLinkModel(tip_)))
+      throw std::runtime_error("TCP must be rigidly attached to the IK solver tip");
+    output_ = node_->create_publisher<Trajectory>("/cobot/tcp_gizmo/controller_command", 10);
+    state_pub_ = node_->create_publisher<iiwa_msgs::msg::GizmoState>("/cobot/tcp_gizmo/state", 1);
+    command_sub_ = node_->create_subscription<iiwa_msgs::msg::GizmoCommand>(
+      "/cobot/tcp_gizmo/command", 1, [this](const iiwa_msgs::msg::GizmoCommand& msg) {
+        try { command(msg); }
+        catch (const std::exception& e) { reject(std::string("target error: ") + e.what()); }
+      });
+    action_client_ = rclcpp_action::create_client<Follow>(node_, "/cobot/tcp_gizmo/follow_joint_trajectory");
+    action_server_ = rclcpp_action::create_server<Follow>(node_, "/iiwa_arm_controller/follow_joint_trajectory",
+      [this](const rclcpp_action::GoalUUID&, std::shared_ptr<const Follow::Goal>) {
+        if (!action_client_->action_server_is_ready()) return rclcpp_action::GoalResponse::REJECT;
+        ++high_goals_count_;
+        gate_.action(true);  // Block low before accepting a public high goal.
+        reason_ = "trajectory action";
+        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+      },
+      [this](const std::shared_ptr<ServerGoal> goal) {
+        const auto it = high_goals_.find(goal->get_goal_id());
+        if (it != high_goals_.end()) {
+          it->second->cancel_requested = true;
+          cancelHigh(it->second);
+        }
+        return rclcpp_action::CancelResponse::ACCEPT;
+      },
+      [this](const std::shared_ptr<ServerGoal> goal) {
+        auto proxy = std::make_shared<ProxyGoal>();
+        proxy->upstream = goal;
+        high_goals_[goal->get_goal_id()] = proxy;
+        after_low_.push_back([this, proxy]() { sendHigh(proxy); });
+        flushHigh();
+      });
+    normal_sub_ = node_->create_subscription<Trajectory>("/iiwa_arm_controller/joint_trajectory", 10,
+      [this](const Trajectory& msg) {
+        // The low action must have a terminal controller result before ANY high
+        // output. DDS publication/cancel acknowledgements alone are insufficient.
+        gate_.normal(trajectoryDeadline(msg));
+        reason_ = "ordinary trajectory";
+        after_low_.push_back([this, msg]() {
+          gate_.normal(trajectoryDeadline(msg));
+          output_->publish(msg);
+        });
+        flushHigh();
+      });
+    priority_ = node_->create_service<Priority>("/cobot/tcp_gizmo/priority",
+      [this](const std::shared_ptr<rmw_request_id_t> header, const Priority::Request::SharedPtr request) {
+        const auto token = ++lease_requests_[request->owner];
+        if (!request->acquire) {
+          gate_.release(request->owner);
+          replyPriority(header, true, "priority released");
+          return;
+        }
+        if (!gate_.acquire(request->owner, request->ttl, steady())) {
+          replyPriority(header, false, "owner and finite TTL in (0, 60] required");
+          return;
+        }
+        reason_ = "waiting for low action to stop";
+        after_low_.push_back([this, header, request, token]() {
+          if (lease_requests_[request->owner] != token) {
+            replyPriority(header, false, "priority request superseded"); return;
+          }
+          // TTL starts at acknowledgement, even if controller completion was
+          // delayed. The pending callback itself prevents any new low command.
+          gate_.acquire(request->owner, request->ttl, steady());
+          replyPriority(header, true, "controller low action stopped; priority granted");
+          reason_ = "motion priority lease";
+        });
+        flushHigh();
+      });
+    timer_ = node_->create_wall_timer(std::chrono::duration<double>(period_), [this]() { tick(); });
+  }
+
+private:
+  template<class T> T param(const std::string& name, const T& value) {
+    if (!node_->has_parameter(name)) return node_->declare_parameter<T>(name, value);
+    return node_->get_parameter(name).get_value<T>();
+  }
+  double positive(const std::string& name, double value) {
+    value = param<double>(name, value);
+    if (!std::isfinite(value) || value <= 0.) throw std::runtime_error("Invalid " + name);
+    return value;
+  }
+  static double steady() {
+    return std::chrono::duration<double>(std::chrono::steady_clock::now().time_since_epoch()).count();
+  }
+  moveit::core::RobotStatePtr current() {
+    auto monitor = scene_->getStateMonitor();
+    if (!monitor || !monitor->haveCompleteState(rclcpp::Duration::from_seconds(state_timeout_))) return {};
+    auto pair = monitor->getCurrentStateAndTime();
+    const double age = (node_->now() - pair.second).seconds();
+    if (age < -.01 || age > state_timeout_) return {};
+    const auto stamp = pair.second.nanoseconds();
+    if (stamp != feedback_stamp_) { feedback_stamp_ = stamp; feedback_received_ = steady(); }
+    if (steady() - feedback_received_ > state_timeout_) return {};
+    pair.first->update();
+    std::vector<double> positions;
+    pair.first->copyJointGroupPositions(group_, positions);
+    if (!std::all_of(positions.begin(), positions.end(), [](double x) { return std::isfinite(x); })) return {};
+    return pair.first;
+  }
+  bool settled(const moveit::core::RobotState& state) {
+    std::vector<double> positions;
+    state.copyJointGroupPositions(group_, positions);
+    const double now = node_->now().seconds();
+    if (previous_.size() != positions.size() || now < previous_time_) {
+      previous_ = positions; previous_time_ = now; stable_since_ = now; return false;
+    }
+    if (now > previous_time_) {
+      double delta = 0.;
+      for (std::size_t i = 0; i < positions.size(); ++i) delta = std::max(delta, std::abs(positions[i] - previous_[i]));
+      if (delta / (now - previous_time_) > .01) stable_since_ = now;
+      previous_ = positions; previous_time_ = now;
+    }
+    return now - stable_since_ >= .15;
+  }
+  double trajectoryDeadline(const Trajectory& msg) const {
+    double start = node_->now().seconds();
+    if (rclcpp::Time(msg.header.stamp).nanoseconds() != 0)
+      start = std::max(start, rclcpp::Time(msg.header.stamp).seconds());
+    double duration = 0.;
+    for (const auto& point : msg.points)
+      duration = std::max(duration, rclcpp::Duration(point.time_from_start).seconds());
+    return start + duration;
+  }
+  void replyPriority(const std::shared_ptr<rmw_request_id_t>& header, bool success, const std::string& message) {
+    Priority::Response response;
+    response.success = success; response.message = message;
+    priority_->send_response(*header, response);
+  }
+  void flushHigh() {
+    if (!low_barrier_.idle()) return;
+    while (!after_low_.empty()) {
+      auto callback = std::move(after_low_.front());
+      after_low_.pop_front();
+      callback();
+    }
+  }
+  void sendLow(const Trajectory& trajectory) {
+    if (!action_client_->action_server_is_ready()) { reject("controller action unavailable"); return; }
+    if (!low_barrier_.begin()) return;
+    Follow::Goal goal;
+    goal.trajectory = trajectory;
+    // One released marker pose is one complete trajectory. The user chose
+    // to finish this trajectory before dispatching a queued ordinary command.
+    // No low CancelGoal request is needed (including the JTC cancel/result race).
+    goal.goal_time_tolerance = rclcpp::Duration::from_seconds(state_timeout_);
+    rclcpp_action::Client<Follow>::SendGoalOptions options;
+    options.goal_response_callback = [this](const ClientGoal::SharedPtr handle) {
+      low_goal_ = handle;
+      low_barrier_.accepted(static_cast<bool>(handle));
+      if (!handle) {
+        gate_.invalidate(); reason_ = "controller rejected low trajectory"; flushHigh();
+      }
+    };
+    options.result_callback = [this](const ClientGoal::WrappedResult& result) {
+      low_goal_.reset(); low_barrier_.terminal();
+      // A new epoch acknowledges completion to the marker and permits a new
+      // gesture. Never redispatch the same goal after a successful result.
+      gate_.invalidate();
+      reason_ = result.code == rclcpp_action::ResultCode::SUCCEEDED ?
+        "target reached" : "controller did not complete TCP trajectory";
+      flushHigh();
+    };
+    try { action_client_->async_send_goal(goal, options); }
+    catch (const std::exception& error) {
+      // An exception leaves delivery uncertain. Fail closed: do not forward a
+      // high command without a terminal controller result for this send.
+      gate_.invalidate(); reason_ = std::string("low action send uncertain: ") + error.what();
+    }
+  }
+  void cancelHigh(const std::shared_ptr<ProxyGoal>& proxy) {
+    if (proxy->downstream && proxy->cancel_requested && !proxy->cancel_sent && !proxy->finished) {
+      proxy->cancel_sent = true;
+      action_client_->async_cancel_goal(proxy->downstream);
+    }
+  }
+  void finishHigh(const std::shared_ptr<ProxyGoal>& proxy, rclcpp_action::ResultCode code,
+                  const std::shared_ptr<Follow::Result>& result) {
+    if (proxy->finished) return;
+    proxy->finished = true;
+    if (proxy->upstream->is_canceling()) proxy->upstream->canceled(result);
+    else if (code == rclcpp_action::ResultCode::SUCCEEDED) proxy->upstream->succeed(result);
+    else proxy->upstream->abort(result);
+    high_goals_.erase(proxy->upstream->get_goal_id());
+    if (high_goals_count_ > 0) --high_goals_count_;
+    gate_.action(high_goals_count_ > 0);
+  }
+  void sendHigh(const std::shared_ptr<ProxyGoal>& proxy) {
+    if (proxy->cancel_requested || proxy->upstream->is_canceling()) {
+      auto result = std::make_shared<Follow::Result>();
+      result->error_string = "canceled before controller dispatch";
+      finishHigh(proxy, rclcpp_action::ResultCode::CANCELED, result); return;
+    }
+    if (!action_client_->action_server_is_ready()) {
+      auto result = std::make_shared<Follow::Result>();
+      result->error_code = Follow::Result::INVALID_GOAL;
+      result->error_string = "controller action unavailable";
+      finishHigh(proxy, rclcpp_action::ResultCode::ABORTED, result); return;
+    }
+    rclcpp_action::Client<Follow>::SendGoalOptions options;
+    options.goal_response_callback = [this, proxy](const ClientGoal::SharedPtr handle) {
+      proxy->downstream = handle;
+      if (!handle) {
+        auto result = std::make_shared<Follow::Result>();
+        result->error_code = Follow::Result::INVALID_GOAL;
+        result->error_string = "controller rejected trajectory";
+        finishHigh(proxy, rclcpp_action::ResultCode::ABORTED, result);
+      } else cancelHigh(proxy);
+    };
+    options.feedback_callback = [proxy](ClientGoal::SharedPtr, const std::shared_ptr<const Follow::Feedback> feedback) {
+      if (!proxy->finished && proxy->upstream->is_active()) proxy->upstream->publish_feedback(std::make_shared<Follow::Feedback>(*feedback));
+    };
+    options.result_callback = [this, proxy](const ClientGoal::WrappedResult& result) {
+      finishHigh(proxy, result.code, result.result);
+    };
+    try { action_client_->async_send_goal(*proxy->upstream->get_goal(), options); }
+    catch (const std::exception& error) {
+      // Keep its high reservation on ambiguous delivery; exposing low motion
+      // here could overwrite a goal that the controller actually received.
+      reason_ = std::string("high action send uncertain: ") + error.what();
+    }
+  }
+  void reject(const std::string& reason) {
+    RCLCPP_WARN(node_->get_logger(), "TCP gizmo rejected target: %s", reason.c_str());
+    gate_.invalidate(); reason_ = reason;
+  }
+  bool safe(moveit::core::RobotState& state, const planning_scene::PlanningSceneConstPtr& scene) {
+    state.update();
+    return state.satisfiesBounds(group_) && !scene->isStateColliding(state, group_name_);
+  }
+  void command(const iiwa_msgs::msg::GizmoCommand& msg) {
+    auto state = current();
+    if (!state) { reject("joint state unavailable or stale"); return; }
+    if (!after_low_.empty() || !gate_.allowed(steady(), node_->now().seconds(), settled(*state))) return;
+    if (msg.epoch != gate_.epoch()) return;
+    if (!low_barrier_.idle()) return;
+    const auto& p = msg.target.pose;
+    const double age = (node_->now() - rclcpp::Time(msg.target.header.stamp)).seconds();
+    Eigen::Quaterniond q(p.orientation.w, p.orientation.x, p.orientation.y, p.orientation.z);
+    Eigen::Vector3d t(p.position.x, p.position.y, p.position.z);
+    if (msg.target.header.frame_id != frame_ || !q.coeffs().allFinite() || !t.allFinite() ||
+        std::abs(q.norm() - 1.) > .01 || age < -.01 || age > command_timeout_) {
+      reject("invalid or stale target"); return;
+    }
+    Eigen::Isometry3d requested = Eigen::Isometry3d::Identity();
+    requested.linear() = q.normalized().toRotationMatrix(); requested.translation() = t;
+    const auto actual = state->getGlobalLinkTransform(frame_).inverse() * state->getGlobalLinkTransform(tcp_);
+    if (!requiresGizmoMotion((requested.translation() - actual.translation()).norm(),
+                            Eigen::Quaterniond(requested.linear()).angularDistance(Eigen::Quaterniond(actual.linear())))) {
+      gate_.invalidate();
+      reason_ = "target reached"; return;
+    }
+    const auto model_target = state->getGlobalLinkTransform(frame_) * requested;
+    // Solve once on release. setFromIK handles the tool's fixed TCP offset.
+    moveit::core::RobotState candidate(*state);
+    planning_scene_monitor::LockedPlanningSceneRO scene(scene_);
+    const bool valid = candidate.setFromIK(group_, model_target, tcp_, .01,
+      [this, &scene](moveit::core::RobotState* s, const moveit::core::JointModelGroup* group, const double* values) {
+        s->setJointGroupPositions(group, values); return safe(*s, scene);
+      });
+    if (!valid || !safe(candidate, scene)) { reject("target unreachable, outside limits or colliding"); return; }
+    // Do not accept approximate IK solutions as reachable targets.
+    const auto reached = candidate.getGlobalLinkTransform(tcp_);
+    if ((reached.translation() - model_target.translation()).norm() > .002 ||
+        Eigen::AngleAxisd(reached.linear().transpose() * model_target.linear()).angle() > .01) {
+      reject("IK solution does not reach target"); return;
+    }
+    candidate.copyJointGroupPositions(group_, ik_target_);
+    gate_.accept(msg.epoch, steady(), command_timeout_);
+    reason_ = "tracking";
+  }
+  void tick() {
+    flushHigh();
+    auto state = current();
+    if (!state) {
+      if (gate_.hasTarget()) reject("joint state unavailable or stale");
+      publishState({}, false, "joint state unavailable or stale"); return;
+    }
+    const bool available = after_low_.empty() && gate_.allowed(steady(), node_->now().seconds(), settled(*state));
+    if (!available) {
+      publishState(state, false, reason_); return;
+    }
+    // command_timeout validates the age at receipt. A released goal is a
+    // committed action and needs no stream of keepalive mouse commands.
+    if (gate_.hasTarget() && low_barrier_.idle()) {
+      try { step(state); }
+      catch (const std::exception& e) { reject(std::string("TCP trajectory error: ") + e.what()); }
+    }
+    publishState(state, true, reason_);
+  }
+  void step(const moveit::core::RobotStatePtr& state) {
+    const auto& names = group_->getVariableNames();
+    if (ik_target_.size() != names.size()) { reject("missing IK target"); return; }
+    std::vector<double> actual;
+    state->copyJointGroupPositions(group_, actual);
+    Eigen::VectorXd delta(names.size());
+    double duration = 2. * period_;
+    double max_travel = 0.;
+    for (std::size_t i = 0; i < names.size(); ++i) {
+      delta[i] = ik_target_[i] - actual[i];
+      const double travel = std::abs(delta[i]);
+      max_travel = std::max(max_travel, travel);
+      const auto& limit = state->getRobotModel()->getVariableBounds(names[i]);
+      // q(u) = q0 + (3u^2 - 2u^3) * delta, with zero endpoint velocities.
+      if (limit.velocity_bounded_)
+        duration = std::max(duration, 1.5 * travel / limit.max_velocity_);
+      if (limit.acceleration_bounded_)
+        duration = std::max(duration, std::sqrt(6. * travel / limit.max_acceleration_));
+    }
+    if (!delta.allFinite() || !std::isfinite(duration)) { reject("invalid trajectory limits"); return; }
+
+    // Validate the complete path, including the actual tool tip. Use forward
+    // Jacobians only to measure Cartesian speed; no inverse/singularity gate.
+    // Densely sample the cubic path and increase duration to obey BOTH TCP
+    // translation and rotation limits throughout it, not just at its endpoints.
+    const Eigen::Vector3d tcp_offset = (state->getGlobalLinkTransform(tip_).inverse() *
+                                        state->getGlobalLinkTransform(tcp_)).translation();
+    const auto* tip_link = state->getRobotModel()->getLinkModel(tip_);
+    const int samples = std::max(100, static_cast<int>(std::ceil(max_travel / .01)));
+    planning_scene_monitor::LockedPlanningSceneRO scene(scene_);
+    moveit::core::RobotState sample(*state);
+    Eigen::MatrixXd jacobian;
+    std::vector<double> positions(actual.size());
+    for (int k = 0; k <= samples; ++k) {
+      const double u = static_cast<double>(k) / samples;
+      const double blend = u * u * (3. - 2. * u);
+      for (std::size_t i = 0; i < positions.size(); ++i) positions[i] = actual[i] + blend * delta[i];
+      sample.setJointGroupPositions(group_, positions);
+      if (!safe(sample, scene)) { reject("TCP trajectory is outside limits or colliding"); return; }
+      if (!sample.getJacobian(group_, tip_link, tcp_offset, jacobian)) {
+        reject("cannot compute tool TCP speed"); return;
+      }
+      const Eigen::VectorXd twist = jacobian * delta * (6. * u * (1. - u));
+      duration = std::max(duration, 1.01 * twist.head<3>().norm() / linear_speed_);
+      duration = std::max(duration, 1.01 * twist.tail<3>().norm() / angular_speed_);
+    }
+    if (!std::isfinite(duration)) { reject("invalid TCP trajectory duration"); return; }
+    Trajectory message;
+    message.joint_names = names;
+    trajectory_msgs::msg::JointTrajectoryPoint start, end;
+    start.positions = actual;
+    start.velocities.assign(actual.size(), 0.);
+    start.time_from_start = rclcpp::Duration::from_seconds(.001);
+    end.positions = ik_target_;
+    end.velocities.assign(actual.size(), 0.);
+    end.time_from_start = rclcpp::Duration::from_seconds(duration + .001);
+    message.points = {start, end};
+    reason_ = "executing released TCP target";
+    sendLow(message);
+  }
+  void publishState(const moveit::core::RobotStatePtr& state, bool enabled, const std::string& reason) {
+    iiwa_msgs::msg::GizmoState message;
+    message.actual.header.frame_id = (state || have_actual_) ? frame_ : "";
+    message.actual.header.stamp = node_->now();
+    message.actual.pose = last_actual_;
+    if (state) {
+      const auto pose = state->getGlobalLinkTransform(frame_).inverse() * state->getGlobalLinkTransform(tcp_);
+      message.actual.pose.position.x = pose.translation().x(); message.actual.pose.position.y = pose.translation().y();
+      message.actual.pose.position.z = pose.translation().z();
+      const Eigen::Quaterniond q(pose.linear());
+      message.actual.pose.orientation.x = q.x(); message.actual.pose.orientation.y = q.y();
+      message.actual.pose.orientation.z = q.z(); message.actual.pose.orientation.w = q.w();
+      last_actual_ = message.actual.pose; have_actual_ = true;
+    }
+    message.enabled = enabled; message.epoch = gate_.epoch(); message.reason = reason;
+    state_pub_->publish(message);
+  }
+  rclcpp::Node::SharedPtr node_;
+  planning_scene_monitor::PlanningSceneMonitorPtr scene_;
+  std::shared_ptr<servo::ParamListener> listener_;
+  const moveit::core::JointModelGroup* group_{};
+  std::string group_name_, tcp_, tip_, frame_, reason_{"ready"};
+  double period_{}, command_timeout_{}, state_timeout_{}, linear_speed_{}, angular_speed_{};
+  GizmoPolicy gate_;
+  std::vector<double> ik_target_;
+  bool have_actual_{false};
+  LowGoalBarrier low_barrier_;
+  ClientGoal::SharedPtr low_goal_;
+  std::deque<std::function<void()>> after_low_;
+  std::map<std::string, std::uint64_t> lease_requests_;
+  std::map<rclcpp_action::GoalUUID, std::shared_ptr<ProxyGoal>> high_goals_;
+  std::size_t high_goals_count_{0};
+  rclcpp_action::Client<Follow>::SharedPtr action_client_;
+  rclcpp_action::Server<Follow>::SharedPtr action_server_;
+  std::vector<double> previous_;
+  double previous_time_{0.}, stable_since_{0.}, feedback_received_{0.};
+  std::int64_t feedback_stamp_{-1};
+  geometry_msgs::msg::Pose last_actual_{};
+  rclcpp::Publisher<Trajectory>::SharedPtr output_;
+  rclcpp::Publisher<iiwa_msgs::msg::GizmoState>::SharedPtr state_pub_;
+  rclcpp::Subscription<iiwa_msgs::msg::GizmoCommand>::SharedPtr command_sub_;
+  rclcpp::Subscription<Trajectory>::SharedPtr normal_sub_;
+  rclcpp::Service<Priority>::SharedPtr priority_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+}  // namespace iiwa_planning
+
+int main(int argc, char** argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<rclcpp::Node>("tcp_gizmo", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
+  try {
+    iiwa_planning::TcpGizmo gizmo(node);
+    // All motion, priority, target and timer callbacks are serialized.
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(node); executor.spin();
+  } catch (const std::exception& error) {
+    RCLCPP_FATAL(node->get_logger(), "%s", error.what()); rclcpp::shutdown(); return 1;
+  }
+  rclcpp::shutdown(); return 0;
+}

src/iiwa_planning/test/gizmo_webots_regression.py

@@ -0,0 +1,232 @@
+"""Manual regression against Webots: release commits exactly one TCP action.
+
+Exercises the marker model with real ROS/controller feedback. Physical mouse
+input is covered separately by the Webots plugin adapter tests.
+"""
+import copy
+import threading
+import time
+import xml.etree.ElementTree as ET
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.parameter_client import AsyncParameterClient
+from rclpy.qos import QoSProfile, DurabilityPolicy
+from action_msgs.msg import GoalStatusArray
+from control_msgs.action import FollowJointTrajectory
+from controller_manager_msgs.srv import ListControllers
+from trajectory_msgs.msg import JointTrajectoryPoint
+from iiwa_msgs.msg import GizmoCommand, GizmoState
+from iiwa_utils.tcp_gizmo import pose_matrix
+from iiwa_utils.tcp_gizmo_model import DragTarget
+from iiwa_planning.motion_priority import MotionPriorityClient
+
+
+def main():
+    rclpy.init()
+    node = rclpy.create_node('gizmo_webots_regression')
+    state = {}
+    drag = DragTarget()
+
+    def capture(msg):
+        state['gizmo'] = msg
+        if msg.actual.header.frame_id:
+            drag.update_feedback(pose_matrix(msg.actual.pose), msg.enabled, msg.epoch)
+
+    node.create_subscription(GizmoState, '/cobot/tcp_gizmo/state', capture, 10)
+    node.create_subscription(
+        GoalStatusArray, '/cobot/tcp_gizmo/follow_joint_trajectory/_action/status',
+        lambda msg: state.update(status=msg),
+        QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL))
+    publisher = node.create_publisher(GizmoCommand, '/cobot/tcp_gizmo/command', 1)
+
+    def pump(duration):
+        end = time.monotonic() + duration
+        while time.monotonic() < end:
+            rclpy.spin_once(node, timeout_sec=.01)
+
+    def wait(predicate, timeout=30.):
+        end = time.monotonic() + timeout
+        while not predicate():
+            assert time.monotonic() < end, f'Timeout: {state.get("gizmo")}'
+            rclpy.spin_once(node, timeout_sec=.01)
+
+    def ids():
+        return {bytes(g.goal_info.goal_id.uuid) for g in state['status'].status_list} if 'status' in state else set()
+
+    def error(goal):
+        diff = np.linalg.inv(pose_matrix(goal.pose)) @ pose_matrix(state['gizmo'].actual.pose)
+        return np.linalg.norm(diff[:3, 3]), Rotation.from_matrix(diff[:3, :3]).magnitude()
+
+    def release(goal):
+        initial = copy.deepcopy(state['gizmo'].actual)
+        epoch = state['gizmo'].epoch
+        before = ids()
+        drag.step(pose_matrix(initial.pose), False)
+        for _ in range(8):
+            shown, command = drag.step(pose_matrix(goal.pose), True)
+            assert command is None, 'Mouse hold must only edit the preview'
+            pump(.02)
+        assert ids() == before, 'Controller received an action before release'
+        assert error(initial)[0] < .001, 'Robot moved while mouse was held'
+        shown, command = drag.step(shown, False)
+        assert command is not None
+        msg = GizmoCommand(target=copy.deepcopy(goal), epoch=epoch)
+        msg.target.header.stamp = state['gizmo'].actual.header.stamp
+        publisher.publish(msg)
+        return epoch, before
+
+    def finish(goal, epoch, before):
+        # No command keepalive: one release must complete even beyond timeout.
+        wait(lambda: state['gizmo'].epoch != epoch)
+        assert state['gizmo'].reason == 'target reached', state['gizmo'].reason
+        wait(lambda: len(ids() - before) == 1, 2.)
+        wait(lambda: error(goal)[0] < .002 and error(goal)[1] < .01, 2.)
+        pump(.15)
+        assert len(ids() - before) == 1, 'A release generated repeated actions'
+        assert not any(g.status in (1, 2, 3) for g in state['status'].status_list)
+        shown, command = drag.step(pose_matrix(goal.pose), False)
+        assert command is None
+        np.testing.assert_allclose(shown, pose_matrix(state['gizmo'].actual.pose), atol=1e-8)
+
+    try:
+        params = AsyncParameterClient(node, '/tcp_gizmo')
+        assert params.wait_for_services(timeout_sec=30.)
+        future = params.get_parameters(['simulation', 'use_sim_time', 'pose_link', 'robot_description_semantic',
+                                        'max_linear_speed'])
+        wait(future.done)
+        values = future.result().values
+        assert values[0].bool_value and values[1].bool_value, 'Simulation only'
+        assert values[2].string_value == 'tcp', 'Gizmo must use active tool TCP'
+        srdf = ET.fromstring(values[3].string_value)
+        controllers = node.create_client(ListControllers, '/controller_manager/list_controllers')
+        assert controllers.wait_for_service(timeout_sec=30.)
+        deadline = time.monotonic() + 30.
+        while True:
+            listing = controllers.call_async(ListControllers.Request())
+            wait(listing.done)
+            if any(c.name == 'iiwa_arm_controller' and c.state == 'active' for c in listing.result().controller):
+                break
+            assert time.monotonic() < deadline
+            pump(.1)
+        wait(lambda: 'gizmo' in state and state['gizmo'].enabled)
+        action = ActionClient(node, FollowJointTrajectory, '/iiwa_arm_controller/follow_joint_trajectory')
+        assert action.wait_for_server(timeout_sec=5.)
+
+        def ordinary(name):
+            joints = srdf.find(f"group_state[@name='{name}']")
+            goal = FollowJointTrajectory.Goal()
+            goal.trajectory.joint_names = [j.attrib['name'] for j in joints]
+            point = JointTrajectoryPoint(positions=[float(j.attrib['value']) for j in joints])
+            point.time_from_start.sec = 3
+            goal.trajectory.points = [point]
+            future = action.send_goal_async(goal)
+            wait(future.done)
+            assert future.result().accepted
+            return future.result().get_result_async()
+
+        done = ordinary('home')
+        wait(done.done)
+        assert done.result().result.error_code == 0
+        wait(lambda: state['gizmo'].enabled)
+        goal = copy.deepcopy(state['gizmo'].actual)
+        goal.pose.position.z -= .02
+        finish(goal, *release(goal))
+        print('PASS: straight posture -> 2 cm descent; no action during drag, exactly one on release', flush=True)
+
+        done = ordinary('work')
+        wait(done.done)
+        assert done.result().result.error_code == 0
+        wait(lambda: state['gizmo'].enabled)
+        for kind in ('translation', 'rotation'):
+            for axis in range(3):
+                for sign in (1., -1.):
+                    goal = copy.deepcopy(state['gizmo'].actual)
+                    if kind == 'translation':
+                        key = ('x', 'y', 'z')[axis]
+                        setattr(goal.pose.position, key, getattr(goal.pose.position, key) + sign * .01)
+                    else:
+                        rotvec = np.zeros(3)
+                        rotvec[axis] = sign * .03
+                        q = (Rotation.from_rotvec(rotvec) * Rotation.from_matrix(pose_matrix(goal.pose)[:3, :3])).as_quat()
+                        goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w = q.tolist()
+                    finish(goal, *release(goal))
+                    print(f'PASS: {kind} axis={axis} sign={sign:+.0f}; exactly one action', flush=True)
+
+        goal = copy.deepcopy(state['gizmo'].actual)
+        goal.pose.position.x = 10.
+        epoch, before = release(goal)
+        wait(lambda: state['gizmo'].epoch != epoch)
+        assert 'unreachable' in state['gizmo'].reason
+        assert ids() == before, 'Invalid target dispatched an action'
+        shown, command = drag.step(pose_matrix(goal.pose), False)
+        np.testing.assert_allclose(shown, pose_matrix(state['gizmo'].actual.pose), atol=1e-8)
+        assert command is None
+        print('PASS: unreachable released target resets marker; no controller action', flush=True)
+
+        goal = copy.deepcopy(state['gizmo'].actual)
+        goal.pose.position.x -= .1
+        epoch, before = release(goal)
+        wait(lambda: len(ids() - before) == 1)
+        low_id = next(iter(ids() - before))
+        assert any(bytes(g.goal_info.goal_id.uuid) == low_id and g.status == 2 for g in state['status'].status_list)
+        done = ordinary('work')
+        # The high request may be accepted upstream, but must not be dispatched
+        # to JTC until the complete gizmo action has succeeded.
+        deadline = time.monotonic() + 20.
+        while not done.done():
+            assert time.monotonic() < deadline, 'Queued ordinary command did not complete'
+            pump(.01)
+            if len(ids() - before) > 1:
+                assert any(bytes(g.goal_info.goal_id.uuid) == low_id and g.status == 4
+                           for g in state['status'].status_list), 'Ordinary motion interrupted gizmo'
+        assert done.result().result.error_code == 0
+        wait(lambda: state['gizmo'].enabled)
+        assert len(ids() - before) == 2
+        goal = copy.deepcopy(state['gizmo'].actual)
+        goal.pose.position.x -= .01
+        finish(goal, *release(goal))
+        print('PASS: ordinary action waits for completed gizmo action; next release works', flush=True)
+
+        # Pose/joints/file planners first request a lease, before planning from
+        # the robot's current state. Exercise their actual client as well.
+        goal = copy.deepcopy(state['gizmo'].actual)
+        goal.pose.position.x -= .1
+        epoch, before = release(goal)
+        wait(lambda: len(ids() - before) == 1)
+        low_id = next(iter(ids() - before))
+        priority = MotionPriorityClient()
+        completed = threading.Event()
+        failures = []
+        def acquire():
+            try:
+                priority.exchange('release-regression', True, 5.)
+            except Exception as error:
+                failures.append(error)
+            finally:
+                completed.set()
+        started = time.monotonic()
+        worker = threading.Thread(target=acquire, daemon=True)
+        worker.start()
+        try:
+            wait(completed.is_set)
+            assert not failures, failures
+            elapsed = time.monotonic() - started
+            assert any(bytes(g.goal_info.goal_id.uuid) == low_id and g.status == 4
+                       for g in state['status'].status_list), 'Lease granted before gizmo completed'
+            if values[4].double_value <= .03:
+                assert elapsed > 2., 'Slow trajectory did not exercise the old 2 s RPC timeout'
+            priority.exchange('release-regression', False, 5.)
+            wait(lambda: state['gizmo'].enabled)
+            print(f'PASS: planner lease waits for full gizmo action ({elapsed:.2f} s)', flush=True)
+        finally:
+            priority.close()
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/iiwa_planning/test/gizmo_webots_speed.py

@@ -0,0 +1,112 @@
+"""Manual Webots speed regression: work -> 10 cm inward TCP translation.
+
+Run against iiwa.launch.py simulate:=true. Checks actual feedback, both peak
+TCP speed and convergence, and prints elapsed simulation/wall time.
+"""
+import copy
+import math
+import time
+import xml.etree.ElementTree as ET
+
+import rclpy
+from rclpy.action import ActionClient
+from rclpy.parameter_client import AsyncParameterClient
+from control_msgs.action import FollowJointTrajectory
+from controller_manager_msgs.srv import ListControllers
+from trajectory_msgs.msg import JointTrajectoryPoint
+from iiwa_msgs.msg import GizmoCommand, GizmoState
+
+
+def main():
+    rclpy.init()
+    node = rclpy.create_node('gizmo_webots_speed')
+    states = []
+    node.create_subscription(GizmoState, '/cobot/tcp_gizmo/state', states.append, 10)
+    publisher = node.create_publisher(GizmoCommand, '/cobot/tcp_gizmo/command', 1)
+
+    def wait(predicate, timeout=30.):
+        deadline = time.monotonic() + timeout
+        while not predicate():
+            assert time.monotonic() < deadline, 'Timed out waiting for simulation'
+            rclpy.spin_once(node, timeout_sec=.01)
+
+    def xyz(state):
+        p = state.actual.pose.position
+        return p.x, p.y, p.z
+
+    def stamp(state):
+        t = state.actual.header.stamp
+        return t.sec + t.nanosec * 1e-9
+
+    try:
+        params = AsyncParameterClient(node, '/tcp_gizmo')
+        assert params.wait_for_services(timeout_sec=30.)
+        future = params.get_parameters([
+            'simulation', 'use_sim_time', 'max_linear_speed', 'robot_description_semantic'])
+        wait(future.done)
+        values = future.result().values
+        assert values[0].bool_value and values[1].bool_value, 'Webots simulation only'
+        speed_limit = values[2].double_value
+        work = ET.fromstring(values[3].string_value).find("group_state[@name='work']")
+        assert work is not None
+        controllers = node.create_client(ListControllers, '/controller_manager/list_controllers')
+        assert controllers.wait_for_service(timeout_sec=30.)
+        deadline = time.monotonic() + 30.
+        while True:
+            listing = controllers.call_async(ListControllers.Request())
+            wait(listing.done)
+            if any(c.name == 'iiwa_arm_controller' and c.state == 'active'
+                   for c in listing.result().controller):
+                break
+            assert time.monotonic() < deadline, 'Controller did not activate'
+            rclpy.spin_once(node, timeout_sec=.1)
+        wait(lambda: states and states[-1].enabled)
+        action = ActionClient(node, FollowJointTrajectory, '/iiwa_arm_controller/follow_joint_trajectory')
+        assert action.wait_for_server(timeout_sec=5.)
+        goal = FollowJointTrajectory.Goal()
+        goal.trajectory.joint_names = [joint.attrib['name'] for joint in work]
+        point = JointTrajectoryPoint(positions=[float(joint.attrib['value']) for joint in work])
+        point.time_from_start.sec = 3
+        goal.trajectory.points = [point]
+        future = action.send_goal_async(goal)
+        wait(future.done)
+        assert future.result().accepted
+        done = future.result().get_result_async()
+        wait(done.done)
+        assert done.result().result.error_code == 0
+        wait(lambda: states[-1].enabled)
+        initial = states[-1]
+        target = copy.deepcopy(initial.actual)
+        target.pose.position.x -= .1
+        start = len(states) - 1
+        epoch = initial.epoch
+        wall_start = time.monotonic()
+        publisher.publish(GizmoCommand(target=target, epoch=epoch))
+        while states[-1].epoch == epoch:
+            assert time.monotonic() - wall_start < 30., 'TCP did not reach 10 cm target'
+            end = time.monotonic() + .025
+            while time.monotonic() < end:
+                rclpy.spin_once(node, timeout_sec=.005)
+        assert states[-1].reason == 'target reached', states[-1].reason
+        wait(lambda: initial.actual.pose.position.x - states[-1].actual.pose.position.x >= .099, 2.)
+        samples = states[start:]
+        speeds = [math.dist(xyz(a), xyz(b)) / (stamp(b) - stamp(a))
+                  for a, b in zip(samples, samples[1:]) if stamp(b) > stamp(a)]
+        elapsed = stamp(samples[-1]) - stamp(initial)
+        peak = max(speeds)
+        distance = math.dist(xyz(initial), xyz(samples[-1]))
+        print(f'limit={speed_limit:.3f} m/s; distance={distance:.6f} m; '
+              f'simulation_time={elapsed:.3f} s; wall_time={time.monotonic() - wall_start:.3f} s; '
+              f'mean={distance / elapsed:.4f} m/s; peak={peak:.4f} m/s', flush=True)
+        assert peak <= speed_limit * 1.05 + .002, 'Measured TCP speed exceeds configured cap'
+        # At 0.50 m/s, a 10 cm work-pose move must not consist of hundreds of
+        # tiny acceleration-limited 64 ms steps. Keep margin for simulator load.
+        if speed_limit >= .5:
+            assert elapsed < 3., f'Artificial short-step speed bottleneck: {elapsed:.3f} s'
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/iiwa_planning/test/test_jtc_interpolation.cpp

@@ -0,0 +1,36 @@
+#include "iiwa_planning/spline_limits.hpp"
+#include <cassert>
+#include <iostream>
+#include <joint_trajectory_controller/trajectory.hpp>
+
+int main() {
+  using namespace iiwa_planning;
+  std::vector<Sample> samples{{0, {0}, {0}, {0}}, {.2, {1}, {0}, {0}}};
+  std::vector<Limit> limits{{-2, 2, 1, 1, 2}};
+  const auto scale = limit_splines(samples, limits);
+  assert(scale > 1);
+  joint_trajectory_controller::Trajectory jtc;
+  trajectory_msgs::msg::JointTrajectoryPoint a, b, out;
+  a.positions = samples[0].position;
+  a.velocities = samples[0].velocity;
+  a.accelerations = samples[0].acceleration;
+  b.positions = samples[1].position;
+  b.velocities = samples[1].velocity;
+  b.accelerations = samples[1].acceleration;
+  const auto finish = rclcpp::Time(static_cast<int64_t>(samples[1].time * 1e9));
+  const auto start = rclcpp::Time(int64_t(0));
+  double prev = 0;
+  const int n = 10000;
+  for (int i = 0; i <= n; ++i) {
+    const auto now = rclcpp::Time(finish.nanoseconds() * i / n);
+    jtc.interpolate_between_points(start, a, finish, b, now, out);
+    assert(std::abs(out.velocities[0]) <= 1.000001);
+    assert(std::abs(out.accelerations[0]) <= 1.000001);
+    if (i)
+      assert(std::abs((out.accelerations[0] - prev) / (samples[1].time / n)) <=
+             2.00001);
+    prev = out.accelerations[0];
+  }
+  std::cout << "Installed JTC interpolation stays within "
+               "velocity/acceleration/jerk limits\n";
+}

src/iiwa_planning/test/test_motion_priority.py

@@ -0,0 +1,185 @@
+import threading
+import pytest
+from iiwa_planning.motion_priority import MotionLease, LeaseError, MotionSerialiser
+from iiwa_planning.motion_priority import wait_response
+
+
+def test_priority_acquisition_waits_for_full_gizmo_trajectory():
+    from concurrent.futures import Future
+    future = Future()
+    timer = threading.Timer(2.1, lambda: future.set_result('granted'))
+    timer.start()
+    try:
+        assert wait_response(future, timeout=None, available=lambda: True) == 'granted'
+    finally:
+        timer.join()
+
+
+def test_waiting_priority_fails_if_server_disappears():
+    from concurrent.futures import Future
+    future = Future()
+    with pytest.raises(LeaseError, match='unavailable'):
+        wait_response(future, timeout=None, available=lambda: False)
+    assert future.cancelled()
+
+
+def test_overlapping_operations_have_independent_owners_and_release():
+    owners = set()
+    def exchange(owner, acquire, ttl):
+        owners.add(owner) if acquire else owners.discard(owner)
+    with MotionLease(exchange) as first:
+        with MotionLease(exchange) as second:
+            assert first.owner != second.owner
+            assert len(owners) == 2
+        assert owners == {first.owner}
+    assert not owners
+
+
+def test_acquisition_fails_closed():
+    def unavailable(*args):
+        raise LeaseError('unavailable')
+    with pytest.raises(LeaseError), MotionLease(unavailable):
+        pytest.fail('motion entered without reservation')
+
+
+def test_renewal_failure_stops_operation():
+    stopped = threading.Event()
+    calls = []
+    def exchange(owner, acquire, ttl):
+        calls.append(acquire)
+        if acquire and len(calls) > 1:
+            raise LeaseError('lost')
+    with pytest.raises(LeaseError):
+        with MotionLease(exchange, on_lost=stopped.set, renew_interval=0.01) as lease:
+            assert stopped.wait(1)
+            lease.check()
+    assert calls[-1] is False
+
+
+def test_release_does_not_mask_original_error():
+    def exchange(owner, acquire, ttl):
+        if not acquire:
+            raise LeaseError('release failed')
+    with pytest.raises(ValueError, match='original'):
+        with MotionLease(exchange):
+            raise ValueError('original')
+
+
+def test_stop_invalidates_plans_and_queued_requests():
+    serial = MotionSerialiser()
+    generation = serial.generation()
+    serial.stop(lambda: None)
+    with pytest.raises(LeaseError):
+        serial.check(generation)
+    serial.check(serial.generation())
+
+
+def test_motion_lock_serializes_workers():
+    serial = MotionSerialiser()
+    entered = threading.Event()
+    finished = threading.Event()
+    with serial.lock:
+        def worker():
+            entered.set()
+            with serial.lock:
+                finished.set()
+        thread = threading.Thread(target=worker)
+        thread.start()
+        assert entered.wait(1)
+        assert not finished.is_set()
+    thread.join(1)
+    assert finished.is_set()
+
+
+def _server_methods():
+    """Exercise real server orchestration without requiring MoveIt or ROS libraries."""
+    import ast
+    from pathlib import Path
+    path = Path(__file__).parents[1] / 'scripts' / 'move_to_pose_server.py'
+    source = ast.parse(path.read_text())
+    cls = next(n for n in source.body if isinstance(n, ast.ClassDef))
+    names = {'_plan_and_execute', '_check_operation', '_guarded', '_lease'}
+    methods = [n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name in names]
+    namespace = {'threading': threading, 'MotionLease': MotionLease,
+                 'PlanRequestParameters': object}
+    exec(compile(ast.Module(body=methods, type_ignores=[]), str(path), 'exec'), namespace)
+    return type('Server', (), {n: namespace[n] for n in names})
+
+
+def _fake_server(plan, execute):
+    from types import SimpleNamespace
+    server = _server_methods()()
+    server._motion_serial = MotionSerialiser()
+    server._operation = threading.local()
+    server._operation.generation = server._motion_serial.generation()
+    server._operation.lease = MotionLease()
+    server._arm = SimpleNamespace(plan=plan)
+    server._moveit = SimpleNamespace(execute=execute)
+    server.get_logger = lambda: SimpleNamespace(error=lambda _: None)
+    return server
+
+
+def test_stop_during_planning_prevents_execution():
+    from types import SimpleNamespace
+    executions = []
+    def plan(**kwargs):
+        server._motion_serial.stop(lambda: None)
+        return SimpleNamespace(trajectory=object())
+    server = _fake_server(plan, lambda *a, **kw: executions.append(a))
+    with pytest.raises(LeaseError, match='stop'):
+        server._plan_and_execute(None, SimpleNamespace(is_cancel_requested=False))
+    assert not executions
+
+
+def test_failed_execution_is_reported_as_failure():
+    from types import SimpleNamespace
+    server = _fake_server(lambda **kw: SimpleNamespace(trajectory=object()),
+                          lambda *a, **kw: False)
+    ok, _ = server._plan_and_execute(None, SimpleNamespace(is_cancel_requested=False))
+    assert ok is False
+
+
+def test_queued_motion_reserves_before_waiting_for_planning_lock():
+    server = _server_methods()()
+    from types import SimpleNamespace
+    reserved = threading.Event()
+    completed = threading.Event()
+    server._motion_serial = MotionSerialiser()
+    server._operation = threading.local()
+    server._gizmo_enabled = True
+    server._stop_execution = lambda: None
+    server._priority = SimpleNamespace(exchange=lambda owner, acquire, ttl:
+                                       reserved.set() if acquire else None)
+    with server._motion_serial.lock:
+        thread = threading.Thread(target=lambda: server._guarded(completed.set))
+        thread.start()
+        assert reserved.wait(1)
+        assert not completed.is_set()
+    thread.join(1)
+    assert completed.is_set()
+
+
+def test_disabled_lease_needs_no_ros_service():
+    with MotionLease() as lease:
+        lease.check()
+
+
+def test_failed_sequence_reports_nonzero_exit_and_releases_resources():
+    import ast
+    from pathlib import Path
+    from types import SimpleNamespace
+    path = Path(__file__).parents[1] / 'scripts' / 'motion_sequence_runner.py'
+    tree = ast.parse(path.read_text())
+    cls = next(n for n in tree.body if isinstance(n, ast.ClassDef))
+    method = next(n for n in cls.body if isinstance(n, ast.FunctionDef) and n.name == 'run')
+    namespace = {'threading': threading, 'MotionLease': MotionLease, 'LeaseError': LeaseError}
+    exec(compile(ast.Module(body=[method], type_ignores=[]), str(path), 'exec'), namespace)
+    stopped, closed, done = threading.Event(), threading.Event(), threading.Event()
+    def unavailable():
+        raise LeaseError('priority unavailable')
+    runner = SimpleNamespace(_discover_priority=unavailable, _priority=None,
+                             _abort_sequence=stopped.set, close_bag=closed.set,
+                             get_logger=lambda: SimpleNamespace(error=lambda _: None))
+    namespace['run'](runner, done)
+    assert stopped.is_set() and closed.is_set() and done.is_set()
+    assert runner.exit_code != 0

src/iiwa_planning/test/test_prepare_trajectory.py

@@ -0,0 +1,29 @@
+import numpy as np
+import pytest
+from iiwa_planning.direct_trajectory import prepare_trajectory
+
+LIMITS = [[-2, 2, 1, 1, 2]]
+
+
+def test_dense_points_are_smooth_and_retimed():
+    t, q, v, a, scale = prepare_trajectory([0, .1, .2], [[0], [.5], [1]], LIMITS)
+    assert scale > 1
+    np.testing.assert_allclose(q, [[0], [.5], [1]])
+    assert abs(v[1][0]) > 0  # do not stop at every CSV waypoint
+    np.testing.assert_allclose([v[0], v[-1], a[0], a[-1]], 0, atol=1e-9)
+    assert t[-1] > .2
+
+
+@pytest.mark.parametrize('times,positions', [([0, 0], [[0], [1]]), ([0, 1], [[0], [float('nan')]]), ([0, 1], [[0], [3]])])
+def test_bad_trajectory_is_rejected(times, positions):
+    with pytest.raises((ValueError, RuntimeError)):
+        prepare_trajectory(times, positions, LIMITS)
+
+
+def test_time_scaling_preserves_quintic_between_waypoints():
+    from scipy.interpolate import BPoly
+    times, positions, velocities, accelerations, scale = prepare_trajectory([0,.1,.2], [[0],[.5],[1]], LIMITS)
+    curve = BPoly.from_derivatives(times, [[q,v,a] for q,v,a in zip(positions,velocities,accelerations)])
+    grid = np.linspace(times[0],times[-1],10001)
+    for order,cap in [(1,1),(2,1),(3,2)]:
+        assert np.max(np.abs(curve(grid,nu=order))) <= cap * 1.000001

src/iiwa_planning/test/test_response_adapter.cpp

@@ -0,0 +1,65 @@
+#include <cassert>
+#include <iostream>
+#include <moveit/planning_interface/planning_response_adapter.hpp>
+#include <moveit/robot_model/robot_model.hpp>
+#include <moveit/robot_trajectory/robot_trajectory.hpp>
+#include <moveit/trajectory_processing/ruckig_traj_smoothing.hpp>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.hpp>
+#include <pluginlib/class_loader.hpp>
+#include <srdfdom/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+int main() {
+  auto urdf = urdf::parseURDF(
+      R"(<robot name="test"><link name="base"/><link name="tip"/><joint name="joint1" type="revolute"><parent link="base"/><child link="tip"/><axis xyz="0 0 1"/><limit lower="-2" upper="2" velocity="1" effort="1"/></joint></robot>)");
+  auto srdf = std::make_shared<srdf::Model>();
+  assert(srdf->initString(
+      *urdf,
+      R"(<robot name="test"><group name="arm"><joint name="joint1"/></group></robot>)"));
+  auto model = std::make_shared<moveit::core::RobotModel>(urdf, srdf);
+  auto bounds = model->getVariableBounds("joint1");
+  bounds.acceleration_bounded_ = bounds.jerk_bounded_ = true;
+  bounds.min_acceleration_ = -1;
+  bounds.max_acceleration_ = 1;
+  bounds.min_jerk_ = -2;
+  bounds.max_jerk_ = 2;
+  model->getJointModel("joint1")->setVariableBounds("joint1", bounds);
+  auto trajectory =
+      std::make_shared<robot_trajectory::RobotTrajectory>(model, "arm");
+  moveit::core::RobotState state(model);
+  state.setToDefaultValues();
+  state.setVariableVelocity("joint1", 0);
+  state.setVariableAcceleration("joint1", 0);
+  trajectory->addSuffixWayPoint(state, 0);
+  state.setVariablePosition("joint1", 1);
+  trajectory->addSuffixWayPoint(state, .2);
+  assert(trajectory_processing::TimeOptimalTrajectoryGeneration()
+             .computeTimeStamps(*trajectory));
+  pluginlib::ClassLoader<planning_interface::PlanningResponseAdapter> loader(
+      "moveit_core", "planning_interface::PlanningResponseAdapter");
+  auto guard = loader.createSharedInstance("iiwa_planning/LimitSplineDynamics");
+  planning_interface::MotionPlanRequest req;
+  req.group_name = "arm";
+  req.max_velocity_scaling_factor = req.max_acceleration_scaling_factor = 1;
+  planning_interface::MotionPlanResponse res;
+  res.trajectory = trajectory;
+  res.error_code = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
+  auto boundary = loader.createSharedInstance("iiwa_planning/SetRestBoundary");
+  req.start_state.joint_state.name = {"joint1"};
+  req.start_state.joint_state.position = {0};
+  req.start_state.joint_state.velocity = {0};
+  boundary->adapt(nullptr, req, res);
+  assert(bool(res));
+  assert(trajectory_processing::RuckigSmoothing::applySmoothing(*trajectory));
+  boundary->adapt(nullptr, req, res);
+  assert(bool(res));
+  guard->adapt(nullptr, req, res);
+  assert(bool(res));
+  assert(trajectory->getDuration() > .2);
+  state.setVariablePosition("joint1", 3);
+  trajectory->addSuffixWayPoint(state, 1);
+  guard->adapt(nullptr, req, res);
+  assert(!bool(res));
+  std::cout << "Installed Ruckig + pluginlib response adapter: success and "
+               "rejection OK\n";
+}

src/iiwa_planning/test/test_spline_limits.cpp

@@ -0,0 +1,58 @@
+#include "iiwa_planning/spline_limits.hpp"
+#include <cassert>
+#include <cmath>
+#include <iostream>
+using namespace iiwa_planning;
+int main() {
+  // Rest-to-rest quintic: endpoint acceleration is zero, interior peak > 5.
+  std::vector<Sample> points{{0, {0}, {0}, {0}}, {1, {1}, {0}, {0}}};
+  std::vector<Limit> limits{{-2, 2, 2, 1, 2}};
+  const double scale = limit_splines(points, limits);
+  assert(scale > 3.0);
+  assert(std::abs(points.back().position[0] - 1) < 1e-12);
+  assert(limit_splines(points, limits) <= 1.000001);
+  // Consistent time scaling keeps the geometric quintic unchanged.
+  auto p = quintic(0, 1, 0, 0, 0, 0, 1);
+  auto q = quintic(0, 1, 0, 0, 0, 0, points.back().time);
+  for (int i = 0; i <= 5; ++i)
+    assert(std::abs(p[i] - q[i]) < 1e-10);
+  bool bad = false;
+  try {
+    std::vector<Sample> s{{0, {0}, {0}, {0}}, {0, {1}, {0}, {0}}};
+    limit_splines(s, limits);
+  } catch (const std::exception &) {
+    bad = true;
+  }
+  assert(bad);
+  bad = false;
+  // Positions are in range but large endpoint velocities overshoot between
+  // them.
+  try {
+    std::vector<Sample> s{{0, {0}, {0}, {0}},
+                          {1, {0}, {12}, {0}},
+                          {2, {0}, {-12}, {0}},
+                          {3, {0}, {0}, {0}}};
+    limit_splines(s, limits);
+  } catch (const std::exception &) {
+    bad = true;
+  }
+  assert(bad);
+  bad = false;
+  try {
+    std::vector<Sample> s{{0, {NAN}, {0}, {0}}, {1, {0}, {0}, {0}}};
+    limit_splines(s, limits);
+  } catch (const std::exception &) {
+    bad = true;
+  }
+  assert(bad);
+  // Non-rest boundaries must not slip past the finite-segment jerk check.
+  bad = false;
+  try {
+    std::vector<Sample> s{{0, {0}, {0}, {1}}, {1, {1}, {0}, {0}}};
+    limit_splines(s, limits);
+  } catch (const std::exception &) {
+    bad = true;
+  }
+  assert(bad);
+  std::cout << "spline limits OK\n";
+}

src/iiwa_planning/test/test_start_state_monitor.cpp

@@ -0,0 +1,41 @@
+#include "iiwa_planning/start_state_monitor.hpp"
+#include <cassert>
+#include <iostream>
+#include <thread>
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<rclcpp::Node>(
+      "iiwa_start_monitor_test", rclcpp::NodeOptions().enable_rosout(false));
+  iiwa_planning::StartStateMonitor monitor;
+  monitor.initialize(node);
+  auto pub = node->create_publisher<sensor_msgs::msg::JointState>(
+      "/joint_states", rclcpp::SensorDataQoS());
+  sensor_msgs::msg::JointState
+      requested; // Normal MoveIt request has no velocities.
+  assert(!monitor.stationary({"joint1"}, requested));
+  sensor_msgs::msg::JointState state;
+  state.name = {"joint1"};
+  state.position = {0};
+  state.velocity = {0};
+  auto publish = [&] {
+    for (int i = 0; i < 30; ++i) {
+      state.header.stamp = node->now();
+      pub->publish(state);
+      rclcpp::spin_some(node);
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+  };
+  publish();
+  assert(monitor.stationary({"joint1"}, requested));
+  state.velocity = {.1};
+  publish();
+  assert(!monitor.stationary({"joint1"}, requested));
+  state.velocity = {0};
+  publish();
+  assert(monitor.stationary({"joint1"}, requested));
+  std::this_thread::sleep_for(std::chrono::milliseconds(550));
+  assert(!monitor.stationary({"joint1"}, requested));
+  rclcpp::shutdown();
+  std::cout << "Position-only MoveIt requests validated using fresh real "
+               "velocity feedback\n";
+}

src/iiwa_planning/test/test_tcp_gizmo_policy.cpp

@@ -0,0 +1,81 @@
+#include <gtest/gtest.h>
+#include <iiwa_planning/tcp_gizmo_policy.hpp>
+using iiwa_planning::GizmoPolicy;
+TEST(GizmoPolicy, LeasesAreBarriersAndExpiredCommandsNeverReplay) {
+  GizmoPolicy gate;
+  EXPECT_TRUE(gate.accept(0, 1., .3));
+  EXPECT_TRUE(gate.acquire("planning", 2., 1.));
+  EXPECT_EQ(gate.epoch(), 1u);
+  EXPECT_FALSE(gate.allowed(1.5, 0., true));
+  EXPECT_FALSE(gate.hasTarget());
+  EXPECT_TRUE(gate.allowed(3., 0., true));
+  EXPECT_FALSE(gate.accept(0, 3., .3));
+  EXPECT_TRUE(gate.accept(1, 3., .3));
+  EXPECT_TRUE(gate.expired(3.31));
+}
+TEST(GizmoPolicy, RenewalAndMultipleOwnersHoldThroughPauses) {
+  GizmoPolicy gate;
+  gate.acquire("a", 2., 0.); gate.acquire("b", 5., 0.);
+  gate.acquire("a", 4., 1.);
+  EXPECT_EQ(gate.epoch(), 2u);
+  gate.release("b");
+  EXPECT_FALSE(gate.allowed(4., 100., true));
+  EXPECT_TRUE(gate.allowed(5.1, 100., true));
+}
+TEST(GizmoPolicy, NormalTrajectoryWaitsForSimulationDeadlineAndSettling) {
+  GizmoPolicy gate;
+  gate.normal(12.);
+  EXPECT_FALSE(gate.allowed(100., 11., true));
+  EXPECT_FALSE(gate.allowed(100., 12.1, false));
+  EXPECT_TRUE(gate.allowed(100., 12.1, true));
+  EXPECT_FALSE(gate.hasTarget());
+}
+TEST(GizmoPolicy, RejectsInvalidLeaseAndBlocksActions) {
+  GizmoPolicy gate;
+  EXPECT_FALSE(gate.acquire("", 1., 0.));
+  EXPECT_FALSE(gate.acquire("x", -1., 0.));
+  gate.action(true);
+  EXPECT_FALSE(gate.allowed(0., 0., true));
+  gate.action(false);
+  EXPECT_FALSE(gate.allowed(0., 0., false));
+  EXPECT_TRUE(gate.allowed(0., 0., true));
+}
+
+TEST(GizmoPolicy, LeaseReleaseRequiresRobotToSettle) {
+  GizmoPolicy gate;
+  gate.acquire("planning", 2., 0.);
+  gate.release("planning");
+  EXPECT_FALSE(gate.allowed(1., 1., false));
+  EXPECT_TRUE(gate.allowed(1., 1., true));
+}
+
+TEST(LowGoalBarrier, DelayedAcceptanceCannotPassHighBarrier) {
+  iiwa_planning::LowGoalBarrier barrier;
+  EXPECT_TRUE(barrier.begin());
+  EXPECT_FALSE(barrier.idle());
+  barrier.accepted(true);
+  EXPECT_FALSE(barrier.idle());
+  // A high-priority request must drain the already accepted low step.
+  EXPECT_FALSE(barrier.begin());
+  barrier.terminal();
+  EXPECT_TRUE(barrier.idle());
+}
+TEST(LowGoalBarrier, RejectedGoalReleasesBarrierWithoutCancellation) {
+  iiwa_planning::LowGoalBarrier barrier;
+  EXPECT_TRUE(barrier.begin());
+  EXPECT_FALSE(barrier.begin());
+  barrier.accepted(false);
+  EXPECT_TRUE(barrier.idle());
+  EXPECT_TRUE(barrier.begin());
+  barrier.accepted(true);
+  EXPECT_FALSE(barrier.idle());
+  barrier.terminal();
+  EXPECT_TRUE(barrier.idle());
+}
+
+TEST(GizmoPolicy, ReachedTargetDoesNotRequestServoMotion) {
+  EXPECT_FALSE(iiwa_planning::requiresGizmoMotion(0., 0.));
+  EXPECT_FALSE(iiwa_planning::requiresGizmoMotion(1e-6, 1e-5));
+  EXPECT_TRUE(iiwa_planning::requiresGizmoMotion(1e-3, 0.));
+  EXPECT_TRUE(iiwa_planning::requiresGizmoMotion(0., .01));
+}

src/iiwa_utils/iiwa_utils/camera_spawner.py

@@ -178,12 +178,19 @@ def build_ros_urdf(cfg: CameraConfig) -> str:
         devices.append(
             f'    <device reference="{cfg.name}" type="Camera">\n'
             f'      <ros>\n'
+            f'        <enabled>false</enabled>\n'
             f'        <topicName>{ros.topic}</topicName>\n'
             f'        <updateRate>{ros.update_rate}</updateRate>\n'
             f'        <alwaysOn>True</alwaysOn>\n'
             f'        <frameName>{cfg.name}_link</frameName>\n'
             f'      </ros>\n'
-            f'    </device>'
+            f'    </device>\n'
+            f'    <plugin type="iiwa_utils.webots_camera.WebotsCamera">\n'
+            f'      <device>{cfg.name}</device>\n'
+            f'      <topicName>{ros.topic}</topicName>\n'
+            f'      <updateRate>{ros.update_rate}</updateRate>\n'
+            f'      <frameName>{cfg.name}_link</frameName>\n'
+            f'    </plugin>'
         )
 
     if cfg.range_finder is not None:
@@ -191,12 +198,20 @@ def build_ros_urdf(cfg: CameraConfig) -> str:
         devices.append(
             f'    <device reference="{cfg.name}_depth" type="RangeFinder">\n'
             f'      <ros>\n'
+            f'        <enabled>false</enabled>\n'
             f'        <topicName>{ros.topic}</topicName>\n'
             f'        <updateRate>{ros.update_rate}</updateRate>\n'
             f'        <alwaysOn>True</alwaysOn>\n'
             f'        <frameName>{cfg.name}_link</frameName>\n'
             f'      </ros>\n'
-            f'    </device>'
+            f'    </device>\n'
+            f'    <plugin type="iiwa_utils.webots_camera.WebotsCamera">\n'
+            f'      <device>{cfg.name}_depth</device>\n'
+            f'      <depth>true</depth>\n'
+            f'      <topicName>{ros.topic}</topicName>\n'
+            f'      <updateRate>{ros.update_rate}</updateRate>\n'
+            f'      <frameName>{cfg.name}_link</frameName>\n'
+            f'    </plugin>'
         )
 
     devices_str = "\n".join(devices)

src/iiwa_utils/iiwa_utils/converter.py

@@ -1,4 +1,6 @@
 import yaml
+import copy
+import math
 import tempfile
 
 from pathlib import Path
@@ -28,9 +30,24 @@ def load_robot_description(
     raise FileNotFoundError(f"Supported file formats: .xacro/.urdf, got: {model_path}")
 
 
+def effective_joint_limits(data: dict) -> dict:
+    """Keep MoveIt's YAML overrides inside the same protective bounds as FRI."""
+    result = copy.deepcopy(data)
+    for name, joint in result['joint_limits'].items():
+        joint['min_position'] = max(joint['min_position'], joint.get('soft_min_position', joint['min_position']))
+        joint['max_position'] = min(joint['max_position'], joint.get('soft_max_position', joint['max_position']))
+        if not all(math.isfinite(joint[key]) for key in ('min_position', 'max_position', 'max_velocity', 'max_acceleration', 'max_jerk')):
+            raise ValueError(f'Non-finite joint limits: {name}')
+        if joint['min_position'] >= joint['max_position'] or any(joint[key] <= 0 for key in ('max_velocity', 'max_acceleration', 'max_jerk')):
+            raise ValueError(f'Invalid joint limits: {name}')
+    return result
+
+
 def wrap_for_ros2_params(yaml_path: str, namespace: str) -> str:
     with open(yaml_path, "r") as f:
         data = yaml.safe_load(f)
+    if namespace == 'robot_description_planning':
+        data = effective_joint_limits(data)
     wrapped = {namespace: {"ros__parameters": data}}
     tmp = tempfile.NamedTemporaryFile(mode="w", suffix=".yaml", delete=False)
     yaml.dump(wrapped, tmp, default_flow_style=False)

src/iiwa_utils/iiwa_utils/object_scene.py

@@ -0,0 +1,196 @@
+"""Validated object configuration and Webots scene generation (metres/radians)."""
+import json
+import math
+import random
+import shlex
+from dataclasses import dataclass, field
+from pathlib import Path
+
+from scipy.spatial.transform import Rotation
+
+
+@dataclass(frozen=True)
+class ObjectModelCfg:
+    path: str
+    rotation_deg: tuple[float, float, float] = (0.0, 0.0, 0.0)
+
+
+@dataclass(frozen=True)
+class ObjectsCfg:
+    enabled: bool = False
+    count: int = 0
+    models: list[ObjectModelCfg] = field(default_factory=list)
+    area: dict[str, list[float]] = field(default_factory=dict)
+
+
+def parse_objects(raw, resolve_path) -> ObjectsCfg:
+    if raw is None:
+        return ObjectsCfg()
+    if not isinstance(raw, dict):
+        raise ValueError('digital_twin.webots.objects must be a mapping')
+    enabled = raw.get('enabled', False)
+    count = raw.get('count', 0)
+    models = raw.get('models', [])
+    area = raw.get('area', {})
+    if type(enabled) is not bool:
+        raise ValueError('objects.enabled must be true or false')
+    if type(count) is not int or count < 0:
+        raise ValueError('objects.count must be an integer >= 0')
+    if not isinstance(models, list):
+        raise ValueError('objects.models must be a list of paths or model mappings')
+    if enabled and count and not models:
+        raise ValueError('objects.models must not be empty when count > 0')
+    if not isinstance(area, dict):
+        raise ValueError('objects.area must be a mapping')
+    parsed_area = {}
+    if area or (enabled and count):
+        for axis in 'xyz':
+            bounds = area.get(axis)
+            if (not isinstance(bounds, (list, tuple)) or len(bounds) != 2
+                    or any(type(v) not in (int, float) or not math.isfinite(v) for v in bounds)
+                    or bounds[0] > bounds[1]):
+                raise ValueError(f'objects.area.{axis} must be [min, max], finite metres, min <= max')
+            parsed_area[axis] = list(map(float, bounds))
+    resolved = []
+    for index, entry in enumerate(models):
+        if isinstance(entry, str):
+            entry = {'path': entry}
+        if not isinstance(entry, dict) or entry.keys() - {'path', 'rotation_deg'}:
+            raise ValueError(f'objects.models[{index}] requires path and optional rotation_deg')
+        path = entry.get('path')
+        angles = entry.get('rotation_deg', (0, 0, 0))
+        if not isinstance(path, str) or not path.strip():
+            raise ValueError(f'objects.models[{index}].path must be a nonempty path')
+        if (not isinstance(angles, (list, tuple)) or len(angles) != 3
+                or any(type(v) not in (int, float) or not math.isfinite(v) for v in angles)):
+            raise ValueError(f'objects.models[{index}].rotation_deg must be finite [X, Y, Z] degrees')
+        resolved.append(ObjectModelCfg(resolve_path(path), tuple(map(float, angles))))
+    if any(Path(model.path).suffix.lower() != '.obj' for model in resolved):
+        raise ValueError('objects.models currently supports Wavefront .obj files')
+    return ObjectsCfg(enabled, count, resolved, parsed_area)
+
+
+def robot_pose(translation, rotation):
+    try:
+        position = tuple(map(float, translation.split()))
+        orient = tuple(map(float, rotation.split()))
+    except (ValueError, AttributeError) as exc:
+        raise ValueError('Robot transform/rotation must be numeric strings') from exc
+    if len(position) != 3 or len(orient) != 4 or not all(map(math.isfinite, position + orient)):
+        raise ValueError('Robot transform needs 3 and rotation 4 finite numbers')
+    length = math.sqrt(sum(v*v for v in orient[:3]))
+    if length == 0:
+        raise ValueError('Robot rotation axis must not be zero')
+    return position, tuple(v / length for v in orient[:3]) + (orient[3],)
+
+
+def world_position(local, translation, rotation):
+    position, orient = robot_pose(translation, rotation)
+    axis, angle = orient[:3], orient[3]
+    c, s = math.cos(angle), math.sin(angle)
+    dot = sum(a*b for a, b in zip(axis, local))
+    cross = (axis[1]*local[2] - axis[2]*local[1],
+             axis[2]*local[0] - axis[0]*local[2],
+             axis[0]*local[1] - axis[1]*local[0])
+    return tuple(position[i] + local[i]*c + cross[i]*s + axis[i]*dot*(1-c) for i in range(3))
+
+
+@dataclass(frozen=True)
+class ObjAsset:
+    path: Path
+    offset: tuple
+    size: tuple
+    textures: tuple
+
+
+def _require_file(path):
+    if not path.is_file():
+        raise ValueError(f'Object asset file does not exist: {path}')
+
+
+def load_obj(path) -> ObjAsset:
+    """Keep original OBJ/MTL/UV data; derive a centred, bottom-aligned origin."""
+    path = Path(path).resolve()
+    _require_file(path)
+    low, high = [math.inf]*3, [-math.inf]*3
+    materials, textures = [], []
+    for line in path.read_text(encoding='utf-8').splitlines():
+        parts = line.split()
+        if not parts:
+            continue
+        if parts[0] == 'v':
+            vertex = tuple(map(float, parts[1:4]))
+            if len(vertex) != 3 or not all(map(math.isfinite, vertex)):
+                raise ValueError(f'Invalid vertex in {path}')
+            low = [min(a, b) for a, b in zip(low, vertex)]
+            high = [max(a, b) for a, b in zip(high, vertex)]
+        elif parts[0] == 'mtllib':
+            materials.extend(path.parent / p for p in shlex.split(line)[1:])
+    size = tuple(b-a for a, b in zip(low, high))
+    if any(not math.isfinite(v) or v <= 0 for v in size):
+        raise ValueError(f'OBJ must have finite, nonzero 3D bounds: {path}')
+    for material in materials:
+        _require_file(material)
+        for line in material.read_text(encoding='utf-8').splitlines():
+            parts = shlex.split(line, comments=True)
+            if parts and (parts[0].lower().startswith('map_') or parts[0].lower() in ('bump', 'disp', 'decal', 'norm')):
+                # Wavefront map options precede the final texture filename.
+                texture = material.parent / parts[-1]
+                _require_file(texture)
+                textures.append(texture)
+    offset = (-(low[0]+high[0])/2, -(low[1]+high[1])/2, -low[2])
+    return ObjAsset(path, offset, size, tuple(textures))
+
+
+def _vector(values):
+    return ' '.join(format(v, '.12g') for v in values)
+
+
+def model_rotation(rotation_deg):
+    """Fixed-axis XYZ degrees: Rz(z) @ Ry(y) @ Rx(x), relative to robot base."""
+    rotvec = Rotation.from_euler('xyz', rotation_deg, degrees=True).as_rotvec()
+    angle = math.sqrt(sum(v*v for v in rotvec))
+    if angle < 1e-12:
+        return (0.0, 0.0, 1.0, 0.0)
+    return tuple(float(v / angle) for v in rotvec) + (angle,)
+
+
+def build_objects(config, translation, rotation, rng=None):
+    if not config.enabled or config.count == 0:
+        return []
+    _, orient = robot_pose(translation, rotation)
+    # Validate every allowed asset before any service request is sent.
+    assets = [(load_obj(model.path), model.rotation_deg) for model in config.models]
+    rng = rng or random.Random()
+    nodes = []
+    for index in range(config.count):
+        asset, angles = rng.choice(assets)
+        model_orient = model_rotation(angles)
+        matrix = Rotation.from_euler('xyz', angles, degrees=True).as_matrix()
+        # Lift the rotated collision box above the spawn anchor. Keep the
+        # original oriented box, rather than inflating it into a world AABB.
+        half_height = sum(abs(matrix[2, i]) * asset.size[i] / 2 for i in range(3))
+        center = (-asset.offset[0], -asset.offset[1],
+                  -asset.offset[2] + asset.size[2] / 2)
+        rotated_center = matrix @ center
+        visual_offset = tuple(-rotated_center[i] + (half_height if i == 2 else 0)
+                              for i in range(3))
+        local = tuple(rng.uniform(*config.area[axis]) for axis in 'xyz')
+        world = world_position(local, translation, rotation)
+        nodes.append(f'''Solid {{
+  name "spawned_object_{index:04d}"
+  translation {_vector(world)}
+  rotation {_vector(orient)}
+  children [ Pose {{
+    translation {_vector(visual_offset)}
+    rotation {_vector(model_orient)}
+    children [ CadShape {{ url [ {json.dumps(str(asset.path))} ] }} ]
+  }} ]
+  boundingObject Pose {{
+    translation 0 0 {half_height:.12g}
+    rotation {_vector(model_orient)}
+    children [ Box {{ size {_vector(asset.size)} }} ]
+  }}
+  physics Physics {{ density -1 mass 0.5 }}
+}}''')
+    return nodes

src/iiwa_utils/iiwa_utils/object_spawner.py

@@ -1,88 +1,50 @@
-# /Ros2Supervisor/spawn_node_from_string / webots_ros2_msgs/srv/SpawnNodeFromString
-# /Ros2Supervisor/spawn_urdf_robot
-
-
-from random import randint
+"""Spawn the configured batch once through the Webots supervisor service."""
+import json
 
 import rclpy
 from rclpy.executors import ExternalShutdownException
 from rclpy.node import Node
-from rclpy.task import Future
 from webots_ros2_msgs.srv import SpawnNodeFromString
 
+from iiwa_utils.object_scene import build_objects, parse_objects
 
-class ObjectSpawner(Node):
-    def __init__(self):
-        super().__init__("object_spawner")
 
-        # параметры передаваемые
-        self.declare_parameter('objects_path', '')
-
-        # переменные
-        self._object_count: int = randint(1, 5)
-        self._spawned_count: int = 0
-        self._call_in_progress: bool = False
-
-        # Реализация
-        self.cli = self.create_client(
-            SpawnNodeFromString, "/Ros2Supervisor/spawn_node_from_string"
-        )
-
-        while not self.cli.wait_for_service(timeout_sec=10):
-            self.get_logger().warning(
-                "service /Ros2Supervisor/spawn_node_from_string  not available, waiting again..."
-            )
-
-        self._timer = self.create_timer(0.1, self.timer_callback)
-
-    # TODO: переделать процесс спавна, должен сформировать все proto файлы, после чего заспавнить все объекты
-    def timer_callback(self):
-        # TODO: здесь должен получать объекты
-        if self._spawned_count >= self._object_count:
-            self.get_logger().info(f"All {self._object_count} objects spawned")
-            self._timer.cancel()
-            return
-
-        if self._call_in_progress:
-            return
-
-        data = 'Solid { name "test_box2" translation 0 1 0.5 children [ Shape { appearance PBRAppearance { baseColor 0.901961 0.380392 0 } geometry Box { size 0.1 0.1 0.1 } } ] boundingObject Box { size 0.1 0.1 0.1 } physics Physics { } }'
-        data = self._create_group(children=[data])
-
-        req = SpawnNodeFromString.Request(data=data, check_fields=True)
-        self.get_logger().info(
-            f"Calling spawn service #{self._spawned_count + 1}/{self._object_count}"
-        )
-        self._call_in_progress = True
-        future = self.cli.call_async(req)
-        future.add_done_callback(self._response_callback)
-
-    def _response_callback(self, future: Future):
-        try:
-            response = future.result()
-            self.get_logger().info(f"Spawn service response: {response}")
-        except Exception as e:
-            self.get_logger().error(f"Service call field: {e}")
-        finally:
-            self._call_in_progress = False
-            self._spawned_count += 1
-
-    # Вспомогательные методы формирования кода для спавна объектов
-    def _create_group(self, children: list[str]) -> str:
-        return f"DEF OBJECTS Group {{children [{" ".join(children)}]}}"
+def spawn_objects(node):
+    raw = json.loads(node.declare_parameter('objects_config', '{}').value)
+    translation = node.declare_parameter('robot_translation', '0 0 0').value
+    rotation = node.declare_parameter('robot_rotation', '0 0 1 0').value
+    config = parse_objects(raw, lambda path: path)
+    objects = build_objects(config, translation, rotation)
+    if not objects:
+        node.get_logger().info('Object spawning disabled or count is zero')
+        return
+    client = node.create_client(SpawnNodeFromString, '/Ros2Supervisor/spawn_node_from_string')
+    if not client.wait_for_service(timeout_sec=60.0):
+        raise RuntimeError('Webots supervisor unavailable; no objects spawned')
+    for index, data in enumerate(objects):
+        future = client.call_async(SpawnNodeFromString.Request(data=data))
+        rclpy.spin_until_future_complete(node, future, timeout_sec=30.0)
+        if not future.done():
+            # A timeout does not prove rejection: retrying could create duplicates.
+            raise RuntimeError(f'Object {index + 1}: spawn timed out; result unknown, no retry')
+        response = future.result()
+        if response is None or not response.success:
+            raise RuntimeError(f'Object {index + 1} rejected; {index}/{len(objects)} spawned')
+    node.get_logger().info(f'All {len(objects)} objects spawned')
 
 
 def main(args=None):
+    rclpy.init(args=args)
+    node = Node('object_spawner')
     try:
-        rclpy.init(args=args)
-        node = ObjectSpawner()
-        rclpy.spin(node=node)
+        spawn_objects(node)
     except (KeyboardInterrupt, ExternalShutdownException):
         pass
     finally:
         node.destroy_node()
-        rclpy.shutdown()
+        if rclpy.ok():
+            rclpy.shutdown()
 
 
-if __name__ == "__main__":
+if __name__ == '__main__':
     main()

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -1,11 +1,13 @@
 import math
 import os
-from dataclasses import dataclass, fields, is_dataclass
+from dataclasses import dataclass, field, fields, is_dataclass
 from typing import Any, Dict, List, Optional, Type, TypeVar
 
 import yaml
 from ament_index_python.packages import get_package_share_directory
 
+from iiwa_utils.object_scene import ObjectsCfg, load_obj, parse_objects, robot_pose
+
 T = TypeVar("T")
 
 
@@ -26,6 +28,7 @@ class WebotsCfg:
     rotation: str
     controller_timer: str
     cameras: List[str]
+    objects: ObjectsCfg = field(default_factory=ObjectsCfg)
 
 
 @dataclass(frozen=True)
@@ -33,10 +36,21 @@ class RvizCfg:
     config: str
 
 
+@dataclass(frozen=True)
+class TcpGizmoCfg:
+    enabled: bool = False
+    publish_period: float = 0.032
+    command_timeout: float = 0.3
+    state_timeout: float = 0.5
+    max_linear_speed: float = 0.10
+    max_angular_speed: float = 0.4
+
+
 @dataclass(frozen=True)
 class DigitalTwinCfg:
     webots: WebotsCfg
     rviz: RvizCfg
+    tcp_gizmo: TcpGizmoCfg = TcpGizmoCfg()
 
 
 @dataclass(frozen=True)
@@ -63,6 +77,8 @@ class PlanningCfg:
     default_frame: str
     default_planner: str
     planning_attempts: int
+    stopped_velocity_tolerance: float = 0.01
+    state_timeout: float = 0.5
 
 
 @dataclass(frozen=True)
@@ -77,6 +93,9 @@ class WebCfg:
     port: int
     endpoints: str    # resolved absolute path to api_endpoints.yaml
     joint_limits: str  # resolved absolute path to joint_limits.yaml
+    trajectory_state_timeout: float = 0.5
+    trajectory_start_tolerance: float = 0.001
+    trajectory_stopped_velocity: float = 0.01
 
 
 @dataclass(frozen=True)
@@ -107,6 +126,24 @@ class SettingsError(RuntimeError):
     pass
 
 
+def parse_tcp_gizmo(raw) -> TcpGizmoCfg:
+    if raw is None:
+        return TcpGizmoCfg()
+    if not isinstance(raw, dict):
+        raise SettingsError('digital_twin.tcp_gizmo must be a mapping')
+    values = {f.name: raw.get(f.name, f.default) for f in fields(TcpGizmoCfg)}
+    if type(values['enabled']) is not bool:
+        raise SettingsError('digital_twin.tcp_gizmo.enabled must be true or false')
+    for key in values.keys() - {'enabled'}:
+        value = values[key]
+        if isinstance(value, bool) or not isinstance(value, (float, int)) or not math.isfinite(value) or value <= 0:
+            raise SettingsError(f'digital_twin.tcp_gizmo.{key} must be finite and positive')
+        values[key] = float(value)
+    if values['command_timeout'] < 2 * values['publish_period']:
+        raise SettingsError('tcp_gizmo.command_timeout must allow at least two publish periods')
+    return TcpGizmoCfg(**values)
+
+
 @dataclass(frozen=True)
 class Settings:
     robot: RobotCfg
@@ -191,6 +228,9 @@ _WEB_DEFAULTS: Dict[str, Any] = {
     "port": 8007,
     "endpoints": "pkg://iiwa_config/config/api_endpoints.yaml",
     "joint_limits": "pkg://iiwa_config/config/moveit/joint_limits.yaml",
+    "trajectory_state_timeout": 0.5,
+    "trajectory_start_tolerance": 0.001,
+    "trajectory_stopped_velocity": 0.01,
 }
 
 
@@ -207,6 +247,9 @@ def _parse_web(raw: Optional[Dict[str, Any]], settings_dir: str) -> WebCfg:
         port=int(get("port")),
         endpoints=resolve_path(str(get("endpoints")), settings_dir),
         joint_limits=resolve_path(str(get("joint_limits")), settings_dir),
+        trajectory_state_timeout=float(get("trajectory_state_timeout")),
+        trajectory_start_tolerance=float(get("trajectory_start_tolerance")),
+        trajectory_stopped_velocity=float(get("trajectory_stopped_velocity")),
     )
 
 
@@ -310,11 +353,19 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
         rotation=str(require(webots_raw, "rotation")),
         controller_timer=str(int(require(webots_raw, "controller_timer"))),
         cameras=cameras,
+        objects=parse_objects(webots_raw.get("objects"),
+                              lambda p: resolve_path(p, settings_dir)),
     )
+    if webots.objects.enabled and webots.objects.count:
+        robot_pose(webots.transform, webots.rotation)
+        if check_files:
+            for model in webots.objects.models:
+                load_obj(model.path)
     rviz = RvizCfg(
         config=resolve_path(str(require(rviz_raw, "config")), settings_dir)
     )
-    digital_twin = DigitalTwinCfg(webots=webots, rviz=rviz)
+    digital_twin = DigitalTwinCfg(webots=webots, rviz=rviz,
+                                 tcp_gizmo=parse_tcp_gizmo(dt_raw.get('tcp_gizmo')))
 
     # controller + moveit
     ctrl_raw = require(raw, "controller")
@@ -342,6 +393,8 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
         default_frame=str(planning_raw.get("default_frame", "base_link")),
         default_planner=str(planning_raw.get("default_planner", "ompl")),
         planning_attempts=int(planning_raw.get("planning_attempts", 3)),
+        stopped_velocity_tolerance=float(planning_raw.get("stopped_velocity_tolerance", 0.01)),
+        state_timeout=float(planning_raw.get("state_timeout", 0.5)),
     )
 
     # tool

src/iiwa_utils/iiwa_utils/tcp_gizmo.py

@@ -0,0 +1,91 @@
+"""Native Webots TCP marker. Loaded as a webots_ros2_driver Python plugin."""
+import time
+
+import numpy as np
+import rclpy
+from scipy.spatial.transform import Rotation
+
+from iiwa_msgs.msg import GizmoCommand, GizmoState
+from iiwa_utils.tcp_gizmo_model import DragTarget, to_webots, webots_pose
+
+
+def pose_matrix(pose):
+    result = np.eye(4)
+    result[:3, 3] = [pose.position.x, pose.position.y, pose.position.z]
+    result[:3, :3] = Rotation.from_quat([
+        pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w,
+    ]).as_matrix()
+    if not np.isfinite(result).all():
+        raise ValueError('Non-finite TCP pose')
+    return result
+
+
+def set_pose(message, matrix):
+    message.position.x, message.position.y, message.position.z = matrix[:3, 3].tolist()
+    q = Rotation.from_matrix(matrix[:3, :3]).as_quat().tolist()
+    message.orientation.x, message.orientation.y, message.orientation.z, message.orientation.w = q
+
+
+class WebotsTcpGizmo:
+    def init(self, webots_node, properties):
+        self._robot = webots_node.robot
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        self._node = rclpy.create_node('tcp_gizmo_webots')
+        self._frame = properties.get('frame', 'base_link')
+        self._mount = webots_pose(
+            [float(x) for x in properties['translation'].split()],
+            [float(x) for x in properties['rotation'].split()],
+        )
+        self._inverse_mount = np.linalg.inv(self._mount)
+        self._state_timeout = float(properties.get('state_timeout', '0.5'))
+        self._drag = DragTarget()
+        self._last_state = 0.0
+        self._last_stamp = None
+        marker = self._robot.getSelf()
+        self._translation = marker.getField('translation')
+        self._rotation = marker.getField('rotation')
+        self._mouse = self._robot.getMouse()
+        self._mouse.enable(int(self._robot.getBasicTimeStep()))
+        self._publisher = self._node.create_publisher(GizmoCommand, '/cobot/tcp_gizmo/command', 1)
+        self._node.create_subscription(GizmoState, '/cobot/tcp_gizmo/state', self._feedback, 1)
+        self._node.get_logger().info('TCP gizmo: position/rotate the marker, then release the mouse to move')
+
+    def _feedback(self, message):
+        # The launch validates the frame: the Webots mount maps base_link to world.
+        if message.actual.header.frame_id != self._frame:
+            self._drag.invalidate()
+            return
+        try:
+            actual = self._mount @ pose_matrix(message.actual.pose)
+        except ValueError:
+            self._drag.invalidate()
+            return
+        self._last_state = time.monotonic()
+        self._last_stamp = message.actual.header.stamp
+        self._drag.update_feedback(actual, message.enabled, message.epoch)
+
+    def step(self):
+        rclpy.spin_once(self._node, timeout_sec=0)
+        if time.monotonic() - self._last_state > self._state_timeout:
+            self._drag.invalidate()
+        try:
+            observed = webots_pose(self._translation.getSFVec3f(), self._rotation.getSFRotation())
+        except ValueError:
+            self._drag.invalidate()
+            return
+        # Webots may not expose a 3-D handle drag through getSelected().  A
+        # command still requires the marker fields themselves to change, so a
+        # mouse press elsewhere cannot start TCP motion.
+        dragging = self._mouse.getState().left
+        shown, target = self._drag.step(observed, dragging)
+        xyz, aa = to_webots(shown)
+        self._translation.setSFVec3f(xyz)
+        self._rotation.setSFRotation(aa)
+        if target is not None:
+            command = GizmoCommand()
+            command.epoch = self._drag.epoch
+            command.target.header.frame_id = self._frame
+            command.target.header.stamp = self._last_stamp
+            set_pose(command.target.pose, self._inverse_mount @ target)
+            self._publisher.publish(command)

src/iiwa_utils/iiwa_utils/tcp_gizmo_model.py

@@ -0,0 +1,94 @@
+"""Webots marker geometry and drag lifecycle, independent of ROS/Webots APIs."""
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+def webots_pose(translation, axis_angle):
+    xyz = np.asarray(translation, dtype=float)
+    aa = np.asarray(axis_angle, dtype=float)
+    if xyz.shape != (3,) or aa.shape != (4,) or not np.isfinite(xyz).all() or not np.isfinite(aa).all():
+        raise ValueError('Pose must contain finite translation and axis-angle')
+    norm = np.linalg.norm(aa[:3])
+    if norm < 1e-12 and abs(aa[3]) > 1e-12:
+        raise ValueError('Rotation axis must be nonzero')
+    result = np.eye(4)
+    result[:3, :3] = Rotation.from_rotvec(aa[:3] * aa[3] / max(norm, 1e-12)).as_matrix()
+    result[:3, 3] = xyz
+    return result
+
+
+def to_webots(matrix):
+    vector = Rotation.from_matrix(matrix[:3, :3]).as_rotvec()
+    angle = np.linalg.norm(vector)
+    axis = vector / angle if angle > 1e-12 else np.array([0., 0., 1.])
+    return matrix[:3, 3].tolist(), [*axis.tolist(), float(angle)]
+
+
+def close_pose(left, right, linear=0.001, angular=0.01):
+    delta = left[:3, :3].T @ right[:3, :3]
+    return (np.linalg.norm(left[:3, 3] - right[:3, 3]) < linear
+            and Rotation.from_matrix(delta).magnitude() < angular)
+
+
+class DragTarget:
+    """Separate mouse edits from marker writes and never replay rejected targets."""
+
+    def __init__(self):
+        self.actual = None
+        self.epoch = None
+        self.enabled = False
+        self.shown = None
+        self.target = None
+        self._reset = True
+        self._wait_release = False
+        self._mouse_down = False
+        self._submitted = False
+
+    def update_feedback(self, actual, enabled, epoch):
+        if epoch != self.epoch or not enabled:
+            self._reset = True
+            self.target = None
+        self.actual = actual.copy()
+        self.enabled = enabled
+        self.epoch = epoch
+
+    def invalidate(self):
+        self.enabled = False
+        self.target = None
+        self._reset = True
+
+    def step(self, observed, mouse_down):
+        was_down = self._mouse_down
+        self._mouse_down = bool(mouse_down)
+        if self.actual is None:
+            self.shown = observed.copy()
+            return self.shown, None
+        if self._reset or not self.enabled:
+            self.target = None
+            self._submitted = False
+            self._wait_release = mouse_down
+            self._reset = False
+            self.shown = self.actual.copy()
+            return self.shown, None
+        if self._wait_release:
+            self._wait_release = mouse_down
+            self.shown = self.actual.copy()
+            return self.shown, None
+        if self._submitted:
+            # A committed goal owns the marker until controller completion (a
+            # new feedback epoch). Do not queue another mouse gesture behind it.
+            self.shown = self.target.copy()
+            return self.shown, None
+        # Supervisor field writes/readbacks are not mouse commands. In
+        # particular, startup synchronisation can read an older marker pose.
+        if mouse_down and self.shown is not None and not close_pose(observed, self.shown, 1e-6, 1e-5):
+            self.target = observed.copy()
+        command = None
+        if was_down and not mouse_down and self.target is not None:
+            if close_pose(self.target, self.actual):
+                self.target = None
+            else:
+                self._submitted = True
+                command = self.target.copy()
+        self.shown = (self.target if self.target is not None else self.actual).copy()
+        return self.shown, command

src/iiwa_utils/iiwa_utils/tcp_gizmo_spawner.py

@@ -0,0 +1,46 @@
+"""Spawn the nonphysical external supervisor marker without editing the world."""
+import rclpy
+from rclpy.node import Node
+from webots_ros2_msgs.srv import SpawnNodeFromString
+
+
+MARKER = '''DEF TCP_GIZMO Robot {
+  name "tcp_gizmo"
+  controller "<extern>"
+  supervisor TRUE
+  children [
+    Shape {
+      appearance PBRAppearance { baseColor 0.1 0.9 0.65 roughness 0.6 metalness 0 }
+      geometry Sphere { radius 0.018 subdivision 2 }
+    }
+    Shape {
+      appearance Appearance { material Material { emissiveColor 1 0.15 0.15 } }
+      geometry IndexedLineSet { coord Coordinate { point [0 0 0, 0.08 0 0] } coordIndex [0 1 -1] }
+    }
+    Shape {
+      appearance Appearance { material Material { emissiveColor 0.15 1 0.15 } }
+      geometry IndexedLineSet { coord Coordinate { point [0 0 0, 0 0.08 0] } coordIndex [0 1 -1] }
+    }
+    Shape {
+      appearance Appearance { material Material { emissiveColor 0.15 0.35 1 } }
+      geometry IndexedLineSet { coord Coordinate { point [0 0 0, 0 0 0.08] } coordIndex [0 1 -1] }
+    }
+  ]
+}'''
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = Node('tcp_gizmo_spawner')
+    try:
+        client = node.create_client(SpawnNodeFromString, '/Ros2Supervisor/spawn_node_from_string')
+        if not client.wait_for_service(timeout_sec=60.0):
+            raise RuntimeError('Webots supervisor unavailable; TCP marker was not spawned')
+        future = client.call_async(SpawnNodeFromString.Request(data=MARKER, check_fields=True))
+        rclpy.spin_until_future_complete(node, future, timeout_sec=30.0)
+        if not future.done() or not future.result().success:
+            raise RuntimeError('Webots could not spawn TCP marker')
+        node.get_logger().info('TCP marker spawned')
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()

src/iiwa_utils/iiwa_utils/webots_camera.py

@@ -0,0 +1,114 @@
+"""Publish Webots camera frames without accumulating timestep rounding drift."""
+import array
+import math
+import sys
+
+import numpy as np
+import rclpy
+from rclpy.qos import QoSProfile, ReliabilityPolicy
+from builtin_interfaces.msg import Time
+from sensor_msgs.msg import CameraInfo, Image, PointCloud2, PointField
+
+
+class FrameSchedule:
+    """Keep deadlines on the requested cadence, with at most one frame per step."""
+
+    def __init__(self, rate, now):
+        if not math.isfinite(rate) or rate <= 0:
+            raise ValueError('Camera update rate must be finite and positive')
+        self.period = 1.0 / rate
+        self.last_time = now
+        self.next_time = now + self.period
+
+    def due(self, now):
+        if now < self.last_time:
+            self.next_time = now + self.period
+        self.last_time = now
+        if now + 1e-9 < self.next_time:
+            return False
+        elapsed = max(0, math.floor((now - self.next_time + 1e-9) / self.period))
+        self.next_time += (elapsed + 1) * self.period
+        return True
+
+
+class WebotsCamera:
+    """One RGB or depth stream; preserves the stock driver's topic suffixes."""
+
+    def init(self, webots_node, properties):
+        self._robot = webots_node.robot
+        name = properties['device']
+        self._device = self._robot.getDevice(name)
+        if self._device is None:
+            raise ValueError(f'Camera device not found: {name}')
+        self._depth = properties.get('depth', 'false') == 'true'
+        rate = float(properties.get('updateRate', '30'))
+        self._schedule = FrameSchedule(rate, self._robot.getTime())
+        timestep = int(self._robot.getBasicTimeStep())
+        if timestep <= 0 or rate * timestep > 1000:
+            raise ValueError('Camera rate exceeds the Webots simulation step rate')
+        self._device.enable(timestep)
+        if not rclpy.ok():
+            rclpy.init(args=None)
+        self._node = rclpy.create_node(f'{name}_publisher')
+        qos = QoSProfile(depth=5, reliability=ReliabilityPolicy.RELIABLE)
+        topic = properties['topicName']
+        suffix = '/image' if self._depth else '/image_color'
+        self._publisher = self._node.create_publisher(Image, topic + suffix, qos)
+        self._info_publisher = self._node.create_publisher(CameraInfo, topic + '/camera_info', qos)
+        self._info = CameraInfo()
+        self._info.header.frame_id = properties['frameName']
+        self._info.width = self._device.getWidth()
+        self._info.height = self._device.getHeight()
+        focal = self._info.width / (2 * math.tan(self._device.getFov() / 2))
+        cx, cy = self._info.width / 2, self._info.height / 2
+        self._info.distortion_model = 'plumb_bob'
+        self._info.d = [0.0] * 5
+        self._info.k = [focal, 0.0, cx, 0.0, focal, cy, 0.0, 0.0, 1.0]
+        self._info.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
+        self._info.p = [focal, 0.0, cx, 0.0, 0.0, focal, cy, 0.0, 0.0, 0.0, 1.0, 0.0]
+        if self._depth:
+            self._cloud_publisher = self._node.create_publisher(
+                PointCloud2, topic + '/point_cloud', qos)
+            u, v = np.meshgrid(np.arange(self._info.width), np.arange(self._info.height))
+            self._rays = np.stack(((u - cx) / focal, (v - cy) / focal,
+                                   np.ones_like(u)), axis=-1).astype(np.float32)
+
+    def step(self):
+        now = self._robot.getTime()
+        if not self._schedule.due(now):
+            return
+        if self._depth:
+            data = array.array('f', self._device.getRangeImage()).tobytes()
+        else:
+            data = self._device.getImage()
+        if data is None:
+            return
+        nanoseconds = round(now * 1_000_000_000)
+        self._info.header.stamp = Time(
+            sec=nanoseconds // 1_000_000_000, nanosec=nanoseconds % 1_000_000_000)
+        message = Image()
+        message.header = self._info.header
+        message.width, message.height = self._info.width, self._info.height
+        message.encoding = '32FC1' if self._depth else 'bgra8'
+        message.is_bigendian = int(sys.byteorder == 'big') if self._depth else 0
+        message.step = message.width * 4
+        message.data = data
+        self._publisher.publish(message)
+        self._info_publisher.publish(self._info)
+        if self._depth and self._cloud_publisher.get_subscription_count():
+            depth = np.frombuffer(data, dtype=np.float32).reshape(message.height, message.width)
+            # Optical coordinates: x right, y down, z forward, all in metres.
+            valid = np.isfinite(depth) & (depth > 0)
+            points = self._rays * np.where(valid, depth, 0.0)[..., None]
+            points[~valid] = np.nan
+            cloud = PointCloud2()
+            cloud.header = message.header
+            cloud.width, cloud.height = message.width, message.height
+            cloud.fields = [PointField(name=name, offset=4 * i,
+                                       datatype=PointField.FLOAT32, count=1)
+                            for i, name in enumerate(('x', 'y', 'z'))]
+            cloud.is_bigendian = bool(message.is_bigendian)
+            cloud.point_step, cloud.row_step = 12, message.width * 12
+            cloud.is_dense = bool(valid.all())
+            cloud.data = points.tobytes()
+            self._cloud_publisher.publish(cloud)

src/iiwa_utils/package.xml

@@ -11,7 +11,13 @@
   <depend>xacro</depend>
   <depend>ament_index_python</depend>
   <depend>webots_ros2_msgs</depend>
+  <exec_depend>webots_ros2_driver</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
   <depend>python3-yaml</depend>
+  <depend>iiwa_msgs</depend>
+  <depend>python3-numpy</depend>
+  <depend>python3-scipy</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/iiwa_utils/setup.py

@@ -12,6 +12,7 @@ setup(
         ('share/' + package_name, ['package.xml']),
     ],
     install_requires=['setuptools'],
+    tests_require=['pytest'],
     zip_safe=True,
     maintainer='daniel',
     maintainer_email='grabardm@ml-dev.ru',
@@ -21,6 +22,7 @@ setup(
         'console_scripts': [
             "object_spawner = iiwa_utils.object_spawner:main",
             "camera_spawner = iiwa_utils.camera_spawner:main",
+            "tcp_gizmo_spawner = iiwa_utils.tcp_gizmo_spawner:main",
         ],
     },
 )

src/iiwa_utils/test/test_camera_timing.py

@@ -0,0 +1,33 @@
+"""Camera cadence must not halve when 30 Hz is sampled on 32 ms steps."""
+import pytest
+
+from iiwa_utils.webots_camera import FrameSchedule
+
+
+def test_30_hz_on_webots_32_ms_steps():
+    schedule = FrameSchedule(30, 0.0)
+    stamps = [tick * 0.032 for tick in range(1, 3126)
+              if schedule.due(tick * 0.032)]
+    assert len(stamps) == 3000
+    assert all(b > a for a, b in zip(stamps, stamps[1:]))
+    assert max(b - a for a, b in zip(stamps, stamps[1:])) <= 0.064001
+
+
+def test_delayed_step_does_not_publish_catchup_burst():
+    schedule = FrameSchedule(30, 0.0)
+    assert schedule.due(1.0)
+    assert not schedule.due(1.0)
+    assert not schedule.due(1.001)
+    assert schedule.due(1.034)
+
+
+def test_time_reset_restarts_schedule():
+    schedule = FrameSchedule(30, 10.0)
+    assert not schedule.due(0.0)
+    assert schedule.due(0.034)
+
+
+@pytest.mark.parametrize('rate', [0, -1, float('nan'), float('inf')])
+def test_invalid_rates_rejected(rate):
+    with pytest.raises(ValueError):
+        FrameSchedule(rate, 0.0)

src/iiwa_utils/test/test_gizmo_settings.py

@@ -0,0 +1,25 @@
+import dataclasses
+
+import pytest
+
+from iiwa_utils import setting_loader as settings
+
+
+def test_old_configuration_does_not_enable_new_input():
+    assert settings.parse_tcp_gizmo(None).enabled is False
+
+
+@pytest.mark.parametrize('value', ['false', 1, [], {'enabled': 'false'},
+                                  {'enabled': True, 'publish_period': 0},
+                                  {'max_linear_speed': float('nan')},
+                                  {'state_timeout': -1}])
+def test_bad_gizmo_settings_are_rejected(value):
+    with pytest.raises(settings.SettingsError):
+        settings.parse_tcp_gizmo(value)
+
+
+def test_partial_config_preserves_defaults():
+    cfg = settings.parse_tcp_gizmo({'enabled': True, 'max_linear_speed': 0.05})
+    assert cfg.enabled
+    assert cfg.max_linear_speed == 0.05
+    assert dataclasses.asdict(cfg)['max_angular_speed'] > 0

src/iiwa_utils/test/test_joint_limits_consistency.py

@@ -0,0 +1,45 @@
+from pathlib import Path
+import math
+import xml.etree.ElementTree as ET
+import yaml
+import xacro
+import xacro.substitution_args
+
+SRC = Path(__file__).resolve().parents[2]
+LIMITS = SRC / 'iiwa_config/config/moveit/joint_limits.yaml'
+
+
+def test_all_seven_axes_share_limits(monkeypatch):
+    monkeypatch.setattr(xacro.substitution_args, '_eval_find', lambda p: str(SRC / p))
+    data = yaml.safe_load(LIMITS.read_text())['joint_limits']
+    for simulate in ('false', 'true'):
+        robot = ET.fromstring(xacro.process_file(
+            str(SRC / 'iiwa_description/urdf/iiwa7.urdf.xacro'),
+            mappings={'simulate': simulate, 'joint_limits_file': str(LIMITS)}).toxml())
+        for i, (degrees, speed) in enumerate(zip([170,120,170,120,170,120,175], [98,98,100,130,140,180,180]), 1):
+            name = f'joint{i}'
+            limit = data[name]
+            physical = robot.find(f"joint[@name='{name}']/limit")
+            assert float(physical.attrib['lower']) == limit['min_position']
+            assert float(physical.attrib['upper']) == limit['max_position']
+            assert float(physical.attrib['velocity']) == limit['max_velocity']
+            assert limit['max_position'] <= math.radians(degrees) + 1e-12
+            assert limit['min_position'] >= -math.radians(degrees) - 1e-12
+            assert limit['max_velocity'] <= math.radians(speed) + 1e-12
+            safety = robot.find(f"joint[@name='{name}']/safety_controller")
+            assert float(safety.attrib['soft_lower_limit']) == limit['soft_min_position']
+            assert float(safety.attrib['soft_upper_limit']) == limit['soft_max_position']
+            command = robot.find(f"ros2_control/joint[@name='{name}']/command_interface")
+            assert float(command.find("param[@name='min']").text) == limit['soft_min_position']
+            assert float(command.find("param[@name='max']").text) == limit['soft_max_position']
+            assert limit['max_acceleration'] > 0 and limit['max_jerk'] > 0
+
+
+def test_planning_limits_keep_soft_margin():
+    from iiwa_utils.converter import effective_joint_limits
+    raw = yaml.safe_load(LIMITS.read_text())
+    effective = effective_joint_limits(raw)
+    for name, joint in raw['joint_limits'].items():
+        assert effective['joint_limits'][name]['min_position'] == joint['soft_min_position']
+        assert effective['joint_limits'][name]['max_position'] == joint['soft_max_position']
+        assert joint['min_position'] < joint['soft_min_position']  # input not mutated

src/iiwa_utils/test/test_object_spawner_service.py

@@ -0,0 +1,30 @@
+"""Exercise our service requests with the installed Jazzy message schema."""
+from types import SimpleNamespace
+from unittest.mock import Mock
+
+import pytest
+
+from iiwa_utils import object_spawner
+
+
+@pytest.mark.parametrize('success,completed', [(True, True), (False, True), (True, False)])
+def test_service_schema_and_failure_stop(monkeypatch, success, completed):
+    node = Mock()
+    node.declare_parameter.side_effect = [SimpleNamespace(value='{}'),
+                                         SimpleNamespace(value='0 0 0'),
+                                         SimpleNamespace(value='0 0 1 0')]
+    monkeypatch.setattr(object_spawner, 'build_objects', lambda *args: ['Solid {}', 'Solid {}'])
+    monkeypatch.setattr(object_spawner.rclpy, 'spin_until_future_complete', lambda *a, **kw: None)
+    client = node.create_client.return_value
+    client.wait_for_service.return_value = True
+    future = client.call_async.return_value
+    future.done.return_value = completed
+    future.result.return_value = SimpleNamespace(success=success)
+    if success and completed:
+        object_spawner.spawn_objects(node)
+        assert client.call_async.call_count == 2
+        assert client.call_async.call_args.args[0].data == 'Solid {}'
+    else:
+        with pytest.raises(RuntimeError):
+            object_spawner.spawn_objects(node)
+        assert client.call_async.call_count == 1

src/iiwa_utils/test/test_object_spawning.py

@@ -0,0 +1,121 @@
+"""Configuration, model placement and textured asset regressions."""
+import math
+import random
+from pathlib import Path
+
+import pytest
+
+from iiwa_utils import object_scene as scene
+
+
+@pytest.fixture
+def model(tmp_path):
+    (tmp_path / 'texture.png').write_bytes(b'texture')
+    (tmp_path / 'hammer.mtl').write_text('newmtl wood\nmap_Kd texture.png\n')
+    path = tmp_path / 'hammer.obj'
+    path.write_text('mtllib hammer.mtl\nv 10 20 30\nv 12 24 36\nf 1 2 1\n')
+    return path
+
+
+def config(model, **updates):
+    raw = dict(enabled=True, count=5, models=[str(model)],
+               area=dict(x=[1, 1], y=[2, 2], z=[3, 3]))
+    raw.update(updates)
+    return scene.parse_objects(raw, lambda p: p)
+
+
+def test_default_disabled():
+    assert not scene.parse_objects(None, lambda p: p).enabled
+
+
+@pytest.mark.parametrize('updates', [
+    {'count': -1}, {'count': True}, {'count': 1.5}, {'enabled': 'true'},
+    {'models': []}, {'models': 'hammer.obj'},
+    {'area': {'x': [2, 1], 'y': [0, 1], 'z': [0, 1]}},
+    {'area': {'x': [0, math.nan], 'y': [0, 1], 'z': [0, 1]}},
+    {'area': {'x': [False, 1], 'y': [0, 1], 'z': [0, 1]}},
+])
+def test_reject_bad_configuration(model, updates):
+    with pytest.raises(ValueError):
+        config(model, **updates)
+
+
+def test_rotation_about_translated_robot():
+    assert scene.world_position((1, 2, 3), '10 20 30', f'0 0 2 {math.pi/2}') == pytest.approx((8, 21, 33))
+    assert scene.world_position((0, 1, 0), '0 0 0', f'1 0 0 {math.pi/2}') == pytest.approx((0, 0, 1))
+
+
+@pytest.mark.parametrize('translation,rotation', [('0 0', '0 0 1 0'), ('0 nan 0', '0 0 1 0'), ('0 0 0', '0 0 0 1')])
+def test_bad_robot_pose(translation, rotation):
+    with pytest.raises(ValueError):
+        scene.world_position((0, 0, 0), translation, rotation)
+
+
+def test_exact_count_repeats_and_centered_geometry(model):
+    nodes = scene.build_objects(config(model), '10 20 30', '0 0 1 0', random.Random(1))
+    assert len(nodes) == 5
+    assert len(set(nodes)) == 5
+    for node in nodes:
+        assert 'translation 11 22 33' in node
+        assert 'CadShape' in node and str(model) in node
+        assert 'translation -11 -22 -30' in node  # XY centered, bottom at local z=0
+        assert 'size 2 4 6' in node
+    assert scene.build_objects(config(model, count=0), '0 0 0', '0 0 1 0') == []
+
+
+def test_missing_texture_fails_before_spawning(model):
+    (model.parent / 'texture.png').unlink()
+    with pytest.raises(ValueError, match='texture'):
+        scene.build_objects(config(model), '0 0 0', '0 0 1 0')
+
+
+def test_repository_models_have_materials_and_textures():
+    root = Path(__file__).resolve().parents[2] / 'iiwa_description/objects/hammer'
+    models = sorted(root.glob('*.obj'))
+    assert len(models) == 5
+    for path in models:
+        asset = scene.load_obj(path)
+        assert all(size > 0 for size in asset.size)
+        assert asset.textures
+
+
+def test_per_model_rotation_and_serialization(model):
+    from dataclasses import asdict
+    cfg = config(model, models=[str(model), {'path': str(model), 'rotation_deg': [90, 0, 180]}])
+    assert cfg.models[0].rotation_deg == (0.0, 0.0, 0.0)
+    assert cfg.models[1].rotation_deg == (90.0, 0.0, 180.0)
+    assert scene.parse_objects(asdict(cfg), lambda p: p) == cfg
+
+
+@pytest.mark.parametrize('entry', [
+    {}, {'path': 12}, {'path': 'a.obj', 'rotation_deg': [0, 0]},
+    {'path': 'a.obj', 'rotation_deg': [True, 0, 0]},
+    {'path': 'a.obj', 'rotation_deg': [0, float('inf'), 0]},
+    {'path': 'a.obj', 'rotation_deg': '90 0 0'},
+    {'path': 'a.obj', 'rotaton_deg': [90, 0, 0]},
+])
+def test_invalid_model_orientation(model, entry):
+    with pytest.raises(ValueError):
+        config(model, models=[entry])
+
+
+def test_quarter_turn_repositions_geometry_and_collision(model):
+    cfg = config(model, count=1, models=[{'path': str(model), 'rotation_deg': [90, 0, 0]}])
+    node = scene.build_objects(cfg, '10 20 30', f'0 0 1 {math.pi/2}')[0]
+    # Anchor (1,2,3) rotates about the ROBOT, not about the model's X axis.
+    assert 'translation 8 21 33' in node
+    # Raw centre (11,22,33) -> (11,-33,22); rotated half-height = 2.
+    assert 'translation -11 33 -20' in node
+    assert 'translation 0 0 2' in node
+    # Rendering AND the original 2x4x6 collision box receive the model rotation.
+    assert node.count('rotation 1 0 0 1.57079632679') == 2
+    assert 'size 2 4 6' in node
+
+
+def test_rotation_order_xyz():
+    # Apply Rx(90), then Ry(0), then Rz(90): x -> y, y -> z, z -> x.
+    orient = scene.model_rotation([90, 0, 90])
+    encoded = ' '.join(map(str, orient))
+    assert scene.world_position((1, 0, 0), '0 0 0', encoded) == pytest.approx((0, 1, 0))
+    assert scene.world_position((0, 1, 0), '0 0 0', encoded) == pytest.approx((0, 0, 1))
+    assert scene.world_position((0, 0, 1), '0 0 0', encoded) == pytest.approx((1, 0, 0))

src/iiwa_utils/test/test_tcp_gizmo.py

@@ -0,0 +1,162 @@
+"""Regressions for coordinate conversion and rejected/stale drag targets."""
+import math
+
+import numpy as np
+import pytest
+
+from iiwa_utils import tcp_gizmo_model as model
+
+
+def pose(x=0.0):
+    result = np.eye(4)
+    result[0, 3] = x
+    return result
+
+
+def test_world_to_base_includes_rotated_robot_mount():
+    mount = model.webots_pose([1, 2, 3], [0, 0, 1, math.pi / 2])
+    target = model.webots_pose([1, 3, 3], [0, 0, 1, math.pi / 2])
+    np.testing.assert_allclose(np.linalg.inv(mount) @ target, pose(1), atol=1e-12)
+
+
+def test_invalid_axis_or_nonfinite_translation_rejected():
+    for xyz, rotation in [([float('nan'), 0, 0], [0, 0, 1, 0]),
+                          ([0, 0, 0], [0, 0, 0, 1])]:
+        with pytest.raises(ValueError):
+            model.webots_pose(xyz, rotation)
+
+
+def test_drag_retains_target_until_actual_tcp_reaches_it():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    shown, command = drag.step(pose(), False)
+    assert command is None
+    shown, command = drag.step(pose(0.1), True)
+    assert command is None
+    drag.update_feedback(pose(0.05), True, 1)
+    shown, command = drag.step(shown, False)
+    np.testing.assert_array_equal(command, pose(0.1))
+    # A release is a single committed goal, not a stream of new commands.
+    shown, command = drag.step(shown, False)
+    assert command is None
+    drag.update_feedback(pose(0.1), True, 1)
+    shown, command = drag.step(shown, False)
+    assert command is None
+    np.testing.assert_array_equal(shown, pose(0.1))
+
+
+def test_rejection_resets_to_actual_and_requires_release_before_new_drag():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    drag.step(pose(0.5), True)
+    drag.update_feedback(pose(0.01), True, 2)
+    shown, command = drag.step(pose(0.5), True)
+    assert command is None
+    np.testing.assert_array_equal(shown, pose(0.01))
+    drag.step(pose(0.8), True)
+    shown, command = drag.step(shown, False)
+    assert command is None
+    shown, command = drag.step(pose(0.02), True)
+    assert command is None
+    shown, command = drag.step(shown, False)
+    np.testing.assert_array_equal(command, pose(0.02))
+
+
+def test_high_priority_motion_discards_old_target_after_completion():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    drag.step(pose(0.5), True)
+    drag.update_feedback(pose(0.2), False, 2)
+    shown, command = drag.step(pose(0.5), False)
+    assert command is None
+    drag.update_feedback(pose(0.3), True, 2)
+    shown, command = drag.step(shown, False)
+    assert command is None
+    np.testing.assert_array_equal(shown, pose(0.3))
+
+
+def test_stale_feedback_cancels_target():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    drag.step(pose(0.5), True)
+    drag.invalidate()
+    shown, command = drag.step(pose(0.5), True)
+    assert command is None
+    np.testing.assert_array_equal(shown, pose())
+
+
+@pytest.mark.parametrize('offset', [0.00001, 0.02, 0.5])
+def test_idle_marker_readback_difference_never_starts_motion(offset):
+    """Delayed Webots field reads or settling are not mouse input."""
+    drag = model.DragTarget()
+    drag.update_feedback(pose(1), True, 1)
+    drag.step(pose(), False)
+    shown, command = drag.step(pose(1 - offset), False)
+    assert command is None
+    np.testing.assert_array_equal(shown, pose(1))
+
+
+def test_idle_tcp_feedback_changes_do_not_turn_into_drag_commands():
+    drag = model.DragTarget()
+    previous = pose()
+    for index in range(100):
+        actual = pose(0.1 + index * 0.002)
+        drag.update_feedback(actual, True, 1)
+        shown, command = drag.step(previous, False)
+        assert command is None
+        np.testing.assert_array_equal(shown, actual)
+        previous = shown
+
+
+def test_readback_after_mouse_release_does_not_replace_active_target():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    drag.step(pose(0.1), True)
+    shown, command = drag.step(pose(0.08), False)
+    np.testing.assert_array_equal(command, pose(0.1))
+
+
+def test_idle_rotated_marker_readback_never_starts_motion():
+    drag = model.DragTarget()
+    actual = model.webots_pose([0.1, 0.2, 0.3], [1, 0, 0, 0.7])
+    drag.update_feedback(actual, True, 1)
+    drag.step(pose(), False)
+    shown, command = drag.step(pose(), False)
+    assert command is None
+    np.testing.assert_allclose(shown, actual)
+
+
+def test_no_motion_during_long_drag_and_only_final_pose_is_committed():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    for index in range(1, 100):
+        shown, command = drag.step(pose(index / 1000), True)
+        assert command is None
+    shown, command = drag.step(shown, False)
+    np.testing.assert_allclose(command, pose(.099))
+    for _ in range(100):
+        shown, command = drag.step(shown, False)
+        assert command is None
+
+
+def test_next_drag_waits_for_controller_completion_epoch():
+    drag = model.DragTarget()
+    drag.update_feedback(pose(), True, 1)
+    drag.step(pose(), False)
+    drag.step(pose(.1), True)
+    drag.step(pose(.1), False)
+    # Looking close enough is not proof that the controller has finished.
+    drag.update_feedback(pose(.1), True, 1)
+    shown, command = drag.step(pose(.2), True)
+    assert command is None
+    drag.update_feedback(pose(.1), True, 2)
+    drag.step(shown, False)
+    shown, command = drag.step(pose(.2), True)
+    assert command is None
+    shown, command = drag.step(shown, False)
+    np.testing.assert_allclose(command, pose(.2))

src/iiwa_utils/test/test_tcp_gizmo_plugin.py

@@ -0,0 +1,112 @@
+"""Exercise the real plugin step with Webots field/mouse adapters."""
+import copy
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+
+from iiwa_msgs.msg import GizmoState
+from iiwa_utils import tcp_gizmo as plugin
+from iiwa_utils.tcp_gizmo_model import DragTarget
+
+
+class Field:
+    def __init__(self, value):
+        self.value = value
+
+    def getSFVec3f(self):
+        return self.value
+
+    getSFRotation = getSFVec3f
+
+    def setSFVec3f(self, value):
+        self.value = value
+
+    setSFRotation = setSFVec3f
+
+
+def make_plugin(monkeypatch):
+    instance = plugin.WebotsTcpGizmo()
+    instance._node = None
+    instance._frame = 'base_link'
+    instance._mount = np.eye(4)
+    instance._inverse_mount = np.eye(4)
+    instance._state_timeout = 0.5
+    instance._drag = DragTarget()
+    instance._last_state = 0
+    instance._last_stamp = None
+    instance._translation = Field([0, 0, 0])
+    instance._rotation = Field([0, 0, 1, 0])
+    mouse = SimpleNamespace(left=False)
+    instance._mouse = SimpleNamespace(getState=lambda: mouse)
+    marker = SimpleNamespace(getId=lambda: 42, getParentNode=lambda: None)
+    selection = SimpleNamespace(node=marker)
+    instance._marker_id = 42
+    instance._robot = SimpleNamespace(getSelected=lambda: selection.node)
+    commands = []
+    instance._publisher = SimpleNamespace(publish=lambda msg: commands.append(copy.deepcopy(msg)))
+    monkeypatch.setattr(plugin.rclpy, 'spin_once', lambda *args, **kwargs: None)
+    return instance, mouse, selection, commands
+
+
+def feedback(instance, epoch=1):
+    state = GizmoState()
+    state.actual.header.frame_id = 'base_link'
+    state.actual.pose.position.x = 0.1
+    state.actual.pose.orientation.w = 1.0
+    state.enabled = True
+    state.epoch = epoch
+    instance._feedback(state)
+
+
+def test_plugin_publishes_nothing_at_startup_even_with_delayed_fields(monkeypatch):
+    instance, mouse, selection, commands = make_plugin(monkeypatch)
+    for _ in range(100):
+        feedback(instance)
+        instance._translation.value = [0, 0, 0]
+        instance.step()
+    assert commands == []
+    assert instance._translation.value == [0.1, 0.0, 0.0]
+
+
+def test_plugin_mouse_drag_sends_command_and_rejection_stops_it(monkeypatch):
+    instance, mouse, selection, commands = make_plugin(monkeypatch)
+    feedback(instance)
+    instance.step()
+    mouse.left = True
+    instance._translation.value = [0.2, 0, 0]
+    instance.step()
+    assert len(commands) == 0
+    mouse.left = False
+    instance.step()
+    assert len(commands) == 1
+    assert commands[0].target.pose.position.x == pytest.approx(0.2)
+    feedback(instance, epoch=2)
+    instance.step()
+    assert len(commands) == 1
+    assert instance._translation.value == [0.1, 0.0, 0.0]
+
+
+def test_drag_still_commands_when_webots_has_no_selected_node(monkeypatch):
+    """The 3-D handle may not be represented as a selected scene-tree node."""
+    instance, mouse, selection, commands = make_plugin(monkeypatch)
+    feedback(instance)
+    instance.step()
+    mouse.left = True
+    selection.node = None
+    instance._translation.value = [0.2, 0, 0]
+    instance.step()
+    assert len(commands) == 0
+    mouse.left = False
+    instance.step()
+    assert len(commands) == 1
+
+
+def test_clicking_other_object_does_not_start_tcp_motion(monkeypatch):
+    instance, mouse, selection, commands = make_plugin(monkeypatch)
+    feedback(instance)
+    instance.step()
+    mouse.left = True
+    selection.node = None
+    instance.step()
+    assert commands == []

src/iiwa_utils/test/test_tool_tcp.py

@@ -0,0 +1,36 @@
+"""TCP must be at the active tool's working end in the visual model."""
+from pathlib import Path
+import xml.etree.ElementTree as ET
+
+import numpy as np
+
+from iiwa_utils.tool_manager import _write_xacro, load_registry
+
+ROOT = Path(__file__).resolve().parents[2]
+
+
+def test_patron_tcp_is_on_the_distal_face_of_its_mesh():
+    tool = ET.parse(ROOT / 'iiwa_description/urdf/tools/patron.xacro').getroot()
+    joint = tool.find("joint[@name='patron_tcp']")
+    assert joint.find('parent').attrib['link'] == 'patron'
+    assert joint.find('child').attrib['link'] == 'tcp'
+    tcp = np.fromstring(joint.find('origin').attrib['xyz'], sep=' ')
+    mesh = ET.parse(ROOT / 'iiwa_description/resource/meshes/tools/patron.dae').getroot()
+    ns = {'c': 'http://www.collada.org/2005/11/COLLADASchema'}
+    assert float(mesh.find('c:asset/c:unit', ns).attrib['meter']) == 1.
+    geometry = mesh.find('.//c:mesh', ns)
+    ref = geometry.find("c:vertices/c:input[@semantic='POSITION']", ns).attrib['source'][1:]
+    values = geometry.find(f"c:source[@id='{ref}']/c:float_array", ns)
+    points = np.fromstring(values.text, sep=' ').reshape(-1, 3)
+    np.testing.assert_allclose(tcp, [0., 0., points[:, 2].max()], atol=1e-6)
+
+
+def test_without_tool_tcp_coincides_with_flange(tmp_path):
+    registry = load_registry(ROOT / 'iiwa_config/config/tools.yaml')
+    assert registry['none']['tcp_link'] == 'tcp'
+    output = tmp_path / 'tool.xacro'
+    _write_xacro(registry['none'], output)
+    joint = ET.parse(output).getroot().find("joint[@name='link_ee_to_tcp']")
+    assert joint.find('parent').attrib['link'] == 'link_ee'
+    assert joint.find('child').attrib['link'] == 'tcp'
+    np.testing.assert_array_equal(np.fromstring(joint.find('origin').attrib['xyz'], sep=' '), [0, 0, 0])

src/iiwa_utils/test/test_webots_camera.py

@@ -0,0 +1,66 @@
+"""Check published pixels, units and calibration with a small device adapter."""
+import array
+import copy
+import math
+from types import SimpleNamespace
+
+import pytest
+import numpy as np
+
+from iiwa_utils import webots_camera as camera
+
+
+@pytest.mark.parametrize('depth', [False, True])
+def test_messages_match_device_and_share_timestamp(monkeypatch, depth):
+    messages = {}
+    now = [0.0]
+    device = SimpleNamespace(
+        enable=lambda period: None,
+        getWidth=lambda: 2, getHeight=lambda: 1,
+        getFov=lambda: math.pi / 2,
+        getImage=lambda: bytes([1, 2, 3, 255, 4, 5, 6, 255]),
+        getRangeImage=lambda: [1.25, float('inf')],
+    )
+    robot = SimpleNamespace(getDevice=lambda name: device,
+                            getBasicTimeStep=lambda: 32, getTime=lambda: now[0])
+
+    def publisher(message_type, topic, qos):
+        messages[topic] = []
+        return SimpleNamespace(publish=lambda msg: messages[topic].append(copy.deepcopy(msg)),
+                               get_subscription_count=lambda: 1)
+
+    monkeypatch.setattr(camera.rclpy, 'ok', lambda: True)
+    monkeypatch.setattr(camera.rclpy, 'create_node',
+                        lambda name: SimpleNamespace(create_publisher=publisher))
+    plugin = camera.WebotsCamera()
+    plugin.init(SimpleNamespace(robot=robot), dict(
+        device='test_camera', depth=str(depth).lower(), updateRate='30',
+        topicName='/test', frameName='test_optical_frame'))
+    for tick in range(1, 101):
+        now[0] = tick * 0.032
+        plugin.step()
+    images = messages['/test/image' if depth else '/test/image_color']
+    infos = messages['/test/camera_info']
+    assert len(images) == len(infos) == 96
+    image, info = images[-1], infos[-1]
+    assert image.header == info.header
+    assert image.header.frame_id == 'test_optical_frame'
+    assert (image.width, image.height, image.step) == (2, 1, 8)
+    assert image.header.stamp.sec == 3
+    assert image.header.stamp.nanosec == 200_000_000
+    assert info.k[0] == pytest.approx(1.0)
+    assert info.k[2] == 1.0
+    assert info.k[5] == 0.5
+    if depth:
+        assert image.encoding == '32FC1'
+        values = array.array('f', image.data.tobytes())
+        assert values[0] == 1.25
+        assert math.isinf(values[1])
+        cloud = messages['/test/point_cloud'][-1]
+        assert cloud.header == image.header
+        xyz = np.frombuffer(cloud.data, dtype=np.float32).reshape(-1, 3)
+        np.testing.assert_allclose(xyz[0], [-1.25, -0.625, 1.25])
+        assert np.isnan(xyz[1]).all()
+    else:
+        assert image.encoding == 'bgra8'
+        assert image.data.tobytes() == device.getImage()

src/iiwa_web/iiwa_web/config_loader.py

@@ -48,7 +48,7 @@ def _resolve_path(package: str, relative: str) -> Path:
         from ament_index_python.packages import get_package_share_directory
         return Path(get_package_share_directory(package)) / relative
     except Exception:
-        src = Path(__file__).parents[3]  # .../src/iiwa_web/iiwa_web/ -> .../src/
+        src = Path(__file__).parents[2]  # .../src/iiwa_web/iiwa_web/ -> .../src/
         return src / package / relative
 
 
@@ -60,7 +60,8 @@ def _parse_joint_limits_data(data: dict) -> tuple[list[str], list[tuple[float, f
     while f"joint{i}" in joints:
         j = joints[f"joint{i}"]
         names.append(f"joint{i}")
-        limits.append((j["min_position"], j["max_position"]))
+        limits.append((max(j["min_position"], j.get("soft_min_position", j["min_position"])),
+                       min(j["max_position"], j.get("soft_max_position", j["max_position"]))))
         i += 1
     return names, limits
 

src/iiwa_web/iiwa_web/main.py

@@ -3,7 +3,7 @@ from contextlib import asynccontextmanager
 
 import rclpy
 import uvicorn
-from fastapi import FastAPI
+from fastapi import FastAPI, APIRouter
 from fastmcp import FastMCP
 from sensor_msgs.msg import JointState
 from std_srvs.srv import Trigger
@@ -13,6 +13,34 @@ from .ros_node import CobotWebNode, get_bridge, set_bridge
 from . import runner, trajectory, positions
 
 
+stop_router = APIRouter(tags=["stop"])
+
+@stop_router.post("/stop", summary="Остановить всё: runner, траекторию и планировщик")
+def stop_all():
+    runner.stop_if_running()
+    trajectory.send_stop_trajectory()
+
+    try:
+        result = get_bridge().call_service(
+            Trigger,
+            "cobot/stop",
+            Trigger.Request()
+        )
+
+        return {
+            "status": "stopped",
+            "success": result.success,
+            "message": result.message,
+        }
+
+    except RuntimeError:
+        return {
+            "status": "stopped",
+            "success": True,
+            "message": "Планировщик не запущен",
+        }
+
+
 def main():
     rclpy.init()
     node = CobotWebNode()
@@ -31,9 +59,10 @@ def main():
     _schema_app.include_router(runner.router)
     _schema_app.include_router(trajectory.router)
     _schema_app.include_router(positions.router)
+    _schema_app.include_router(stop_router)
 
     mcp = FastMCP.from_fastapi(app=_schema_app)
-    mcp_http = mcp.http_app(path='/mcp')
+    mcp_http = mcp.http_app(path='/')
 
     @asynccontextmanager
     async def lifespan(_: FastAPI):
@@ -46,18 +75,9 @@ def main():
     app.include_router(runner.router)
     app.include_router(trajectory.router)
     app.include_router(positions.router)
+    app.include_router(stop_router)
     app.mount("/mcp", mcp_http)
 
-    @app.post("/stop", tags=["stop"], summary="Остановить всё: runner, траекторию и планировщик")
-    def stop_all():
-        runner.stop_if_running()
-        trajectory.send_stop_trajectory()
-        try:
-            result = get_bridge().call_service(Trigger, "cobot/stop", Trigger.Request())
-            return {"status": "stopped", "success": result.success, "message": result.message}
-        except RuntimeError:
-            return {"status": "stopped", "success": True, "message": "Планировщик не запущен"}
-
     uvicorn.run(app, host=host, port=port)
 
 

src/iiwa_web/iiwa_web/ros_node.py

@@ -15,8 +15,12 @@ class CobotWebNode(Node):
         self.declare_parameter('port', 8007)
         self.declare_parameter('endpoints_path', '')
         self.declare_parameter('joint_limits_path', '')
+        self.declare_parameter('trajectory_state_timeout', 0.5)
+        self.declare_parameter('trajectory_start_tolerance', 0.001)
+        self.declare_parameter('trajectory_stopped_velocity', 0.01)
 
         self._topic_cache: dict = {}
+        self._topic_received: dict = {}
         self._pub_registry: dict = {}
         self._service_clients: dict = {}
         self._action_clients: dict = {}
@@ -47,9 +51,15 @@ class CobotWebNode(Node):
         with self._lock:
             return self._topic_cache.get(topic_name)
 
+    def get_latest_with_age(self, topic_name: str):
+        with self._lock:
+            return (self._topic_cache.get(topic_name),
+                    time.monotonic() - self._topic_received.get(topic_name, float('-inf')))
+
     def _handle_message(self, topic_name: str, msg):
         with self._lock:
             self._topic_cache[topic_name] = msg
+            self._topic_received[topic_name] = time.monotonic()
 
     def call_service(self, srv_type, srv_name: str, request, timeout: float = 5.0):
         if srv_name not in self._service_clients:

src/iiwa_web/iiwa_web/trajectory.py

@@ -1,5 +1,11 @@
 import csv
 import io
+import math
+import threading
+import time
+import yaml
+from pathlib import Path
+from iiwa_planning.direct_trajectory import prepare_trajectory
 from collections import deque
 from datetime import datetime
 from builtin_interfaces.msg import Duration
@@ -7,7 +13,7 @@ from fastapi import APIRouter, File, HTTPException, Query, UploadFile
 from pydantic import BaseModel, Field
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 
-from .config_loader import load_joint_limits, load_joint_names
+from .config_loader import load_joint_limits, load_joint_names, _resolve_path
 from .ros_node import get_bridge
 
 router = APIRouter(prefix="/trajectory", tags=["trajectory"])
@@ -15,6 +21,10 @@ router = APIRouter(prefix="/trajectory", tags=["trajectory"])
 TOPIC = "/iiwa_arm_controller/joint_trajectory"
 JOINT_NAMES = load_joint_names()
 N_JOINTS = len(JOINT_NAMES)
+_prepare_lock = threading.Lock()
+_admission_lock = threading.Lock()
+_generation = 0
+_direct_until = 0.0
 
 _log_lines: deque[str] = deque(maxlen=300)
 
@@ -24,13 +34,13 @@ def _log(msg: str) -> None:
 
 
 def _to_duration(seconds: float) -> Duration:
-    sec = int(seconds)
-    nanosec = int(round((seconds - sec) * 1e9))
+    total_ns = round(seconds * 1e9)
+    sec, nanosec = divmod(total_ns, 1_000_000_000)
     return Duration(sec=sec, nanosec=nanosec)
 
 
 def _validate_limits(points: list[list[float]]) -> None:
-    limits = load_joint_limits()
+    limits = load_joint_limits(path=get_bridge().get_parameter("joint_limits_path").value or None)
     for row_idx, positions in enumerate(points):
         for j, (pos, (lo, hi)) in enumerate(zip(positions, limits)):
             if not (lo <= pos <= hi):
@@ -52,6 +62,81 @@ def _build_msg(rows: list[tuple[list[float], float]]) -> JointTrajectory:
     return msg
 
 
+def _stationary_state():
+    bridge = get_bridge()
+    state, age = bridge.get_latest_with_age('/joint_states')
+    timeout = bridge.get_parameter('trajectory_state_timeout').value
+    tolerance = bridge.get_parameter('trajectory_start_tolerance').value
+    stopped = bridge.get_parameter('trajectory_stopped_velocity').value
+    if any(not math.isfinite(x) or x <= 0 for x in (timeout, tolerance, stopped)):
+        raise HTTPException(503, 'Invalid direct trajectory monitoring parameters')
+    if state is None or age > timeout:
+        raise HTTPException(409, 'Fresh joint_states required before sending a trajectory')
+    stamp = state.header.stamp.sec + state.header.stamp.nanosec * 1e-9
+    source_age = bridge.get_clock().now().nanoseconds * 1e-9 - stamp
+    if not math.isfinite(source_age) or not 0 <= source_age <= timeout:
+        raise HTTPException(409, 'joint_states timestamp is stale or in the future')
+    try:
+        indexes = [list(state.name).index(name) for name in JOINT_NAMES]
+        current = [state.position[i] for i in indexes]
+        velocity = [state.velocity[i] for i in indexes]
+    except (ValueError, IndexError):
+        raise HTTPException(409, 'Complete named position and velocity feedback required')
+    if any(not math.isfinite(x) for x in current + velocity) or any(abs(v) > stopped for v in velocity):
+        raise HTTPException(409, 'Direct trajectories must start from a stationary robot')
+    return current, tolerance
+
+
+def _build_safe_msg(rows):
+    bridge = get_bridge()
+    current, tolerance = _stationary_state()
+    path = bridge.get_parameter('joint_limits_path').value
+    resolved = Path(path) if path else _resolve_path('iiwa_config', 'config/moveit/joint_limits.yaml')
+    data = yaml.safe_load(resolved.read_text())['joint_limits']
+    caps = []
+    for name in JOINT_NAMES:
+        j = data[name]
+        caps.append([max(j['min_position'], j.get('soft_min_position', j['min_position'])),
+                     min(j['max_position'], j.get('soft_max_position', j['max_position'])),
+                     j['max_velocity'], j['max_acceleration'], j['max_jerk']])
+    rows = [(list(q), float(t)) for q, t in rows]
+    if rows[0][1] > 0:
+        rows.insert(0, (current, 0.0))
+    elif rows[0][1] == 0 and max(abs(a-b) for a,b in zip(rows[0][0],current)) > tolerance:
+        raise HTTPException(422, 'A waypoint at t=0 must match the current joint positions')
+    else:
+        rows[0] = (current, rows[0][1])
+    try:
+        times, positions, velocities, accelerations, scale = prepare_trajectory(
+            [t for _, t in rows], [q for q, _ in rows], caps)
+    except (ValueError, RuntimeError) as exc:
+        raise HTTPException(422, str(exc)) from exc
+    msg = _build_msg(list(zip(positions, times)))
+    for point, v, a in zip(msg.points, velocities, accelerations):
+        point.velocities = v
+        point.accelerations = a
+    return msg, times[-1], scale
+
+
+def _send_rows(rows):
+    global _direct_until
+    with _prepare_lock:
+        with _admission_lock:
+            generation = _generation
+            if time.monotonic() < _direct_until:
+                raise HTTPException(409, 'A direct trajectory is still active; stop it before replacing it')
+        msg, duration, scale = _build_safe_msg(rows)
+        with _admission_lock:
+            if generation != _generation:
+                raise HTTPException(409, 'Trajectory preparation was canceled by stop')
+            current, tolerance = _stationary_state()
+            if max(abs(a-b) for a,b in zip(current, msg.points[0].positions)) > tolerance:
+                raise HTTPException(409, 'Robot moved during trajectory preparation; retry from its new state')
+            _publish(msg)
+            _direct_until = time.monotonic() + duration + get_bridge().get_parameter('trajectory_state_timeout').value
+        return msg, duration, scale
+
+
 def _publish(msg: JointTrajectory) -> None:
     get_bridge().publish(TOPIC, JointTrajectory, msg)
 
@@ -80,9 +165,9 @@ def send_trajectory(req: SendRequest):
     if req.validate_limits:
         _validate_limits([r[0] for r in rows])
 
-    _publish(_build_msg(rows))
+    msg, duration, time_scale = _send_rows(rows)
     _log(f"[send] {len(rows)} точек, t_end={rows[-1][1]:.2f}с")
-    return {"status": "sent", "points": len(rows)}
+    return {"status": "sent", "points": len(msg.points), "duration": duration, "time_scale": time_scale}
 
 
 @router.post("/send_csv", summary="Загрузить CSV и отправить траекторию")
@@ -166,26 +251,30 @@ async def send_csv_trajectory(
     if validate_limits:
         _validate_limits([r[0] for r in rows])
 
-    _publish(_build_msg(rows))
+    msg, duration, time_scale = _send_rows(rows)
     _log(f"[csv] {file.filename} → {len(rows)} точек, t_end={rows[-1][1]:.2f}с")
-    return {"status": "sent", "points": len(rows), "filename": file.filename}
+    return {"status": "sent", "points": len(msg.points), "filename": file.filename, "duration": duration, "time_scale": time_scale}
 
 
 def send_stop_trajectory() -> None:
-    """Publish a hold-position (or empty) trajectory to freeze joint motion."""
-    bridge = get_bridge()
-    joint_states = bridge.get_latest("/joint_states")
-    if joint_states is not None and len(joint_states.position) >= N_JOINTS:
-        current_positions = list(joint_states.position[:N_JOINTS])
-        hold_msg = _build_msg([(current_positions, 0.5)])
-        _publish(hold_msg)
-        _log("[stop] отправлена точка удержания текущей позиции")
-    else:
-        msg = JointTrajectory()
-        msg.joint_names = JOINT_NAMES
-        _publish(msg)
-        _log("[stop] joint_states недоступны, отправлена пустая траектория")
-
+    """Cancel preparation and publish hold; malformed feedback falls back to cancellation."""
+    global _generation, _direct_until
+    with _admission_lock:
+        _generation += 1
+        _direct_until = 0.0
+        bridge = get_bridge()
+        state = bridge.get_latest('/joint_states')
+        try:
+            current = [state.position[list(state.name).index(name)] for name in JOINT_NAMES]
+            if not all(math.isfinite(x) for x in current):
+                raise ValueError('Non-finite feedback')
+            _publish(_build_msg([(current, 0.5)]))
+            _log('[stop] отправлена точка удержания текущей позиции')
+        except (AttributeError, ValueError, IndexError):
+            msg = JointTrajectory()
+            msg.joint_names = JOINT_NAMES
+            _publish(msg)
+            _log('[stop] отправлена пустая траектория: неполная обратная связь')
 
 
 @router.get("/logs", summary="Последние лог-записи траекторного модуля")

src/iiwa_web/package.xml

@@ -14,6 +14,8 @@
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>tf2_py</exec_depend>
   <exec_depend>moveit_msgs</exec_depend>
+  <exec_depend>iiwa_planning</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/iiwa_web/setup.py

@@ -19,6 +19,7 @@ setup(
         'python-multipart',
         'fastmcp',
     ],
+    tests_require=['pytest'],
     zip_safe=True,
     maintainer='daniel',
     maintainer_email='grabardm@ml-dev.ru',

src/iiwa_web/test/test_trajectory_admission.py

@@ -0,0 +1,75 @@
+import time
+from pathlib import Path
+from types import SimpleNamespace
+import pytest
+from sensor_msgs.msg import JointState
+from fastapi import HTTPException
+from iiwa_web import trajectory as api
+
+
+@pytest.fixture
+def bridge(monkeypatch):
+    msg = JointState()
+    msg.name = [f'joint{i}' for i in range(7,0,-1)]
+    msg.position = [0.] * 7
+    msg.velocity = [0.] * 7
+    msg.header.stamp.sec = 100
+    params = dict(trajectory_state_timeout=.5, trajectory_start_tolerance=.001,
+                  trajectory_stopped_velocity=.01,
+                  joint_limits_path=str(Path(__file__).resolve().parents[2] / 'iiwa_config/config/moveit/joint_limits.yaml'))
+    published = []
+    node = SimpleNamespace(
+        state=msg, age=0., published=published,
+        get_parameter=lambda key: SimpleNamespace(value=params[key]),
+        get_clock=lambda: SimpleNamespace(now=lambda: SimpleNamespace(nanoseconds=100_000_000_000)),
+        publish=lambda topic, typ, payload: published.append(payload))
+    node.get_latest = lambda topic: node.state
+    node.get_latest_with_age = lambda topic: (node.state, node.age)
+    monkeypatch.setattr(api, 'get_bridge', lambda: node)
+    monkeypatch.setattr(api, '_direct_until', 0.)
+    return node
+
+
+def test_stale_and_moving_state_not_published(bridge):
+    for age, speed in [(1.,0.),(0.,.2)]:
+        bridge.age=age; bridge.state.velocity[0]=speed
+        with pytest.raises(HTTPException): api._send_rows([([.1]*7,1.)])
+    assert not bridge.published
+
+
+def test_normal_preparation_and_duplicate_admission(bridge):
+    msg, duration, scale = api._send_rows([([.1]*7,.1)])
+    assert duration > .1 and scale > 1
+    assert list(msg.points[0].positions) == [0.]*7
+    assert list(msg.points[-1].positions) == [.1]*7
+    assert len(msg.points[1].accelerations) == 7
+    with pytest.raises(HTTPException): api._send_rows([([.2]*7,1.)])
+    assert len(bridge.published) == 1
+
+
+def test_state_rechecked_after_preparation(bridge, monkeypatch):
+    original=api.prepare_trajectory
+    def prepare(*args):
+        result=original(*args)
+        bridge.state.position[0]=.02
+        return result
+    monkeypatch.setattr(api,'prepare_trajectory',prepare)
+    with pytest.raises(HTTPException): api._send_rows([([.1]*7,1.)])
+    assert not bridge.published
+
+
+def test_stop_cancels_inflight_preparation(bridge, monkeypatch):
+    original=api.prepare_trajectory
+    def prepare(*args):
+        result=original(*args)
+        api.send_stop_trajectory()
+        return result
+    monkeypatch.setattr(api,'prepare_trajectory',prepare)
+    with pytest.raises(HTTPException): api._send_rows([([.1]*7,1.)])
+    assert len(bridge.published)==1  # stop only
+
+
+def test_stop_handles_missing_named_feedback(bridge):
+    bridge.state.name=[]
+    api.send_stop_trajectory()
+    assert len(bridge.published)==1 and not bridge.published[0].points