feat: add release-to-move TCP gizmo control in Webots

Add a native Webots marker for setting the active tool's TCP position and
orientation. Dragging edits the preview; releasing the mouse submits one
complete, checked joint trajectory with configured TCP speed limits.
Unreachable targets reset the marker to the current TCP.

Preserve coordinate, joint, and control-file commands through motion
reservations and a controller action proxy. Give the gizmo lowest priority
and finish its committed trajectory before dispatching queued regular
commands, including waits longer than the former two-second service timeout.

Place the patron TCP at its working end, 55.2 mm along local Z, and use the
flange TCP when no tool is attached. Add digital_twin.tcp_gizmo configuration,
ROS interfaces, launch integration, and regression coverage.

Document operation, configuration, tool attachment, speed limits, and
troubleshooting in both Russian and English.

Validation: 42 Python tests and 8 C++ tests passed. Webots regressions covered
one action per release, translation and rotation with and without a tool,
unreachable targets, and ordinary commands waiting for gizmo completion.
This commit is contained in:
Даниил Грабарь
2026-09-17 17:11:53 +03:00
parent 9f0f05df87
commit 3a5bc28bba
40 changed files with 2373 additions and 70 deletions
@@ -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,27 @@ digital_twin:
| `webots.cameras` | Список YAML-конфигов подключённых камер |
| `rviz.config` | Путь к конфигурации RViz |
### Перемещение за 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 +116,8 @@ tool:
Список доступных инструментов находится в файле `src/iiwa_config/config/tools.yaml`. Для добавления нового инструмента необходимо описать его там, после чего указать имя в `tool.active`.
Применяйте выбор через `cobot robot-setup`: мастер обновляет описание инструмента и SRDF. После смены инструмента пересоберите проект и перезапустите симуляцию.
---
## Блок `planning` — планирование движений