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:
Даниил Грабарь
@@ -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,34 @@
|
||||
- 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.
|
||||
|
||||
@@ -1,8 +1,5 @@
|
||||
# Симуляция (Webots)
|
||||
|
||||
!!! info "Раздел в разработке"
|
||||
Подробное описание работы в симуляторе готовится.
|
||||
|
||||
**Webots** — симулятор роботов с открытым исходным кодом. В LWC используется как цифровой двойник робота KUKA LBR IIWA 7: позволяет разрабатывать и отлаживать алгоритмы управления без доступа к физическому роботу.
|
||||
|
||||
## Ключевые особенности
|
||||
@@ -11,8 +8,34 @@
|
||||
- Мир симуляции задаётся в `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
|
||||
|
||||
@@ -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,28 @@ digital_twin:
|
||||
| `webots.cameras` | List of YAML configurations for connected cameras |
|
||||
| `rviz.config` | Path to the RViz configuration |
|
||||
|
||||
### 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 +115,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
|
||||
|
||||
@@ -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` — планирование движений
|
||||
|
||||
Reference in New Issue
Block a user