diff --git a/.gitignore b/.gitignore index a2fbf38..2cd034d 100644 --- a/.gitignore +++ b/.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 diff --git a/cobot-setting.yaml b/cobot-setting.yaml index 8cf012f..36a2e2c 100644 --- a/cobot-setting.yaml +++ b/cobot-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 diff --git a/doc/lwc-doc/docs/getting-started/concepts/simulation.en.md b/doc/lwc-doc/docs/getting-started/concepts/simulation.en.md index 04f4660..03043f0 100644 --- a/doc/lwc-doc/docs/getting-started/concepts/simulation.en.md +++ b/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,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. diff --git a/doc/lwc-doc/docs/getting-started/concepts/simulation.md b/doc/lwc-doc/docs/getting-started/concepts/simulation.md index 6795ad7..70da487 100644 --- a/doc/lwc-doc/docs/getting-started/concepts/simulation.md +++ b/doc/lwc-doc/docs/getting-started/concepts/simulation.md @@ -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 diff --git a/doc/lwc-doc/docs/getting-started/configuration.en.md b/doc/lwc-doc/docs/getting-started/configuration.en.md index f92e684..9587251 100644 --- a/doc/lwc-doc/docs/getting-started/configuration.en.md +++ b/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,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 diff --git a/doc/lwc-doc/docs/getting-started/configuration.md b/doc/lwc-doc/docs/getting-started/configuration.md index c99e264..5a2acc4 100644 --- a/doc/lwc-doc/docs/getting-started/configuration.md +++ b/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,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` — планирование движений diff --git a/rosdep.yaml b/rosdep.yaml index 54c02dc..1f58779 100644 --- a/rosdep.yaml +++ b/rosdep.yaml @@ -18,3 +18,22 @@ 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] diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index c9abd26..e772700 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/src/iiwa_bringup/launch/iiwa.launch.py @@ -90,6 +90,7 @@ 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(), }.items(), )) diff --git a/src/iiwa_bringup/launch/supported/moveit_nodes.py b/src/iiwa_bringup/launch/supported/moveit_nodes.py index ea78412..def2d18 100644 --- a/src/iiwa_bringup/launch/supported/moveit_nodes.py +++ b/src/iiwa_bringup/launch/supported/moveit_nodes.py @@ -1,5 +1,6 @@ from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder +from .tcp_gizmo_nodes import make_tcp_gizmo_nodes def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool): @@ -46,8 +47,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)] diff --git a/src/iiwa_bringup/launch/supported/tcp_gizmo_nodes.py b/src/iiwa_bringup/launch/supported/tcp_gizmo_nodes.py new file mode 100644 index 0000000..b29a018 --- /dev/null +++ b/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]), + ] diff --git a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py index 018ab88..cd8b738 100644 --- a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py +++ b/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, @@ -64,6 +65,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) diff --git a/src/iiwa_bringup/package.xml b/src/iiwa_bringup/package.xml index 01f4001..7e3b2ed 100644 --- a/src/iiwa_bringup/package.xml +++ b/src/iiwa_bringup/package.xml @@ -11,6 +11,7 @@ iiwa_utils + iiwa_planning iiwa_config iiwa_description foxglove_bridge diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index cc6b634..9a7cda4 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/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 diff --git a/src/iiwa_config/config/tools.yaml b/src/iiwa_config/config/tools.yaml index 8f3e014..c1d7a36 100644 --- a/src/iiwa_config/config/tools.yaml +++ b/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] - diff --git a/src/iiwa_description/urdf/tools/patron.xacro b/src/iiwa_description/urdf/tools/patron.xacro index 4469d96..59448e4 100644 --- a/src/iiwa_description/urdf/tools/patron.xacro +++ b/src/iiwa_description/urdf/tools/patron.xacro @@ -107,7 +107,8 @@ - + + @@ -174,4 +175,4 @@ - \ No newline at end of file + diff --git a/src/iiwa_msgs/CMakeLists.txt b/src/iiwa_msgs/CMakeLists.txt index 313fcef..190078a 100644 --- a/src/iiwa_msgs/CMakeLists.txt +++ b/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" diff --git a/src/iiwa_msgs/msg/GizmoCommand.msg b/src/iiwa_msgs/msg/GizmoCommand.msg new file mode 100644 index 0000000..5ca1352 --- /dev/null +++ b/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 diff --git a/src/iiwa_msgs/msg/GizmoState.msg b/src/iiwa_msgs/msg/GizmoState.msg new file mode 100644 index 0000000..c39434b --- /dev/null +++ b/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 diff --git a/src/iiwa_msgs/srv/MotionPriority.srv b/src/iiwa_msgs/srv/MotionPriority.srv new file mode 100644 index 0000000..6c6d50e --- /dev/null +++ b/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 diff --git a/src/iiwa_planning/CMakeLists.txt b/src/iiwa_planning/CMakeLists.txt index 8a4ad33..fb916f2 100644 --- a/src/iiwa_planning/CMakeLists.txt +++ b/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,16 @@ 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) add_executable(motion_planning_cpp src/motion_planning_cpp.cpp) target_include_directories(motion_planning_cpp PUBLIC include) @@ -33,6 +45,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 +68,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() diff --git a/src/iiwa_planning/iiwa_planning/motion_priority.py b/src/iiwa_planning/iiwa_planning/motion_priority.py new file mode 100644 index 0000000..bbc45d1 --- /dev/null +++ b/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() diff --git a/src/iiwa_planning/include/iiwa_planning/tcp_gizmo_policy.hpp b/src/iiwa_planning/include/iiwa_planning/tcp_gizmo_policy.hpp new file mode 100644 index 0000000..39e0b40 --- /dev/null +++ b/src/iiwa_planning/include/iiwa_planning/tcp_gizmo_policy.hpp @@ -0,0 +1,65 @@ +#pragma once +#include +#include +#include +#include + +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 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 diff --git a/src/iiwa_planning/package.xml b/src/iiwa_planning/package.xml index 0be8b87..0d70d4e 100644 --- a/src/iiwa_planning/package.xml +++ b/src/iiwa_planning/package.xml @@ -11,6 +11,8 @@ ament_cmake_python rclcpp + rclcpp_action + control_msgs rclpy moveit_ros_planning_interface moveit_core @@ -25,9 +27,19 @@ tf_transformations python3-scipy iiwa_msgs + moveit_servo + trajectory_msgs + action_msgs + rcl_interfaces + tf2_eigen ament_lint_auto + ament_cmake_gtest + ament_cmake_pytest ament_lint_common + iiwa_utils + controller_manager_msgs + python3-numpy ament_cmake diff --git a/src/iiwa_planning/scripts/motion_sequence_runner.py b/src/iiwa_planning/scripts/motion_sequence_runner.py index 50fc822..212dc91 100644 --- a/src/iiwa_planning/scripts/motion_sequence_runner.py +++ b/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()) diff --git a/src/iiwa_planning/scripts/move_to_pose_server.py b/src/iiwa_planning/scripts/move_to_pose_server.py index 1f33f09..ff805a1 100644 --- a/src/iiwa_planning/scripts/move_to_pose_server.py +++ b/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__": diff --git a/src/iiwa_planning/src/tcp_gizmo.cpp b/src/iiwa_planning/src/tcp_gizmo.cpp new file mode 100644 index 0000000..da436b9 --- /dev/null +++ b/src/iiwa_planning/src/tcp_gizmo.cpp @@ -0,0 +1,471 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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; + using ServerGoal = rclcpp_action::ServerGoalHandle; + struct ProxyGoal { + std::shared_ptr 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("enabled", false) || !param("simulation", false)) + throw std::runtime_error("TCP gizmo requires enabled=true and simulation=true"); + group_name_ = param("planning_group", "iiwa_arm"); + tcp_ = param("pose_link", "tcp"); + frame_ = param("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(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("/cobot/tcp_gizmo/controller_command", 10); + state_pub_ = node_->create_publisher("/cobot/tcp_gizmo/state", 1); + command_sub_ = node_->create_subscription( + "/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(node_, "/cobot/tcp_gizmo/follow_joint_trajectory"); + action_server_ = rclcpp_action::create_server(node_, "/iiwa_arm_controller/follow_joint_trajectory", + [this](const rclcpp_action::GoalUUID&, std::shared_ptr) { + 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 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 goal) { + auto proxy = std::make_shared(); + proxy->upstream = goal; + high_goals_[goal->get_goal_id()] = proxy; + after_low_.push_back([this, proxy]() { sendHigh(proxy); }); + flushHigh(); + }); + normal_sub_ = node_->create_subscription("/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("/cobot/tcp_gizmo/priority", + [this](const std::shared_ptr 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(period_), [this]() { tick(); }); + } + +private: + template T param(const std::string& name, const T& value) { + if (!node_->has_parameter(name)) return node_->declare_parameter(name, value); + return node_->get_parameter(name).get_value(); + } + double positive(const std::string& name, double value) { + value = param(name, value); + if (!std::isfinite(value) || value <= 0.) throw std::runtime_error("Invalid " + name); + return value; + } + static double steady() { + return std::chrono::duration(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 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 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& 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::SendGoalOptions options; + options.goal_response_callback = [this](const ClientGoal::SharedPtr handle) { + low_goal_ = handle; + low_barrier_.accepted(static_cast(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& 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& proxy, rclcpp_action::ResultCode code, + const std::shared_ptr& 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& proxy) { + if (proxy->cancel_requested || proxy->upstream->is_canceling()) { + auto result = std::make_shared(); + 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(); + result->error_code = Follow::Result::INVALID_GOAL; + result->error_string = "controller action unavailable"; + finishHigh(proxy, rclcpp_action::ResultCode::ABORTED, result); return; + } + rclcpp_action::Client::SendGoalOptions options; + options.goal_response_callback = [this, proxy](const ClientGoal::SharedPtr handle) { + proxy->downstream = handle; + if (!handle) { + auto result = std::make_shared(); + 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 feedback) { + if (!proxy->finished && proxy->upstream->is_active()) proxy->upstream->publish_feedback(std::make_shared(*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 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(std::ceil(max_travel / .01))); + planning_scene_monitor::LockedPlanningSceneRO scene(scene_); + moveit::core::RobotState sample(*state); + Eigen::MatrixXd jacobian; + std::vector positions(actual.size()); + for (int k = 0; k <= samples; ++k) { + const double u = static_cast(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 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 ik_target_; + bool have_actual_{false}; + LowGoalBarrier low_barrier_; + ClientGoal::SharedPtr low_goal_; + std::deque> after_low_; + std::map lease_requests_; + std::map> high_goals_; + std::size_t high_goals_count_{0}; + rclcpp_action::Client::SharedPtr action_client_; + rclcpp_action::Server::SharedPtr action_server_; + std::vector 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::SharedPtr output_; + rclcpp::Publisher::SharedPtr state_pub_; + rclcpp::Subscription::SharedPtr command_sub_; + rclcpp::Subscription::SharedPtr normal_sub_; + rclcpp::Service::SharedPtr priority_; + rclcpp::TimerBase::SharedPtr timer_; +}; +} // namespace iiwa_planning + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared("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; +} diff --git a/src/iiwa_planning/test/gizmo_webots_regression.py b/src/iiwa_planning/test/gizmo_webots_regression.py new file mode 100644 index 0000000..a355e5e --- /dev/null +++ b/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() diff --git a/src/iiwa_planning/test/gizmo_webots_speed.py b/src/iiwa_planning/test/gizmo_webots_speed.py new file mode 100644 index 0000000..96783e1 --- /dev/null +++ b/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() diff --git a/src/iiwa_planning/test/test_motion_priority.py b/src/iiwa_planning/test/test_motion_priority.py new file mode 100644 index 0000000..b2df57e --- /dev/null +++ b/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 diff --git a/src/iiwa_planning/test/test_tcp_gizmo_policy.cpp b/src/iiwa_planning/test/test_tcp_gizmo_policy.cpp new file mode 100644 index 0000000..5e0e81d --- /dev/null +++ b/src/iiwa_planning/test/test_tcp_gizmo_policy.cpp @@ -0,0 +1,81 @@ +#include +#include +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)); +} diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index f31f917..1c804c8 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/src/iiwa_utils/iiwa_utils/setting_loader.py @@ -33,10 +33,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) @@ -107,6 +118,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 @@ -314,7 +343,8 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings: 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") diff --git a/src/iiwa_utils/iiwa_utils/tcp_gizmo.py b/src/iiwa_utils/iiwa_utils/tcp_gizmo.py new file mode 100644 index 0000000..e8c8eac --- /dev/null +++ b/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) diff --git a/src/iiwa_utils/iiwa_utils/tcp_gizmo_model.py b/src/iiwa_utils/iiwa_utils/tcp_gizmo_model.py new file mode 100644 index 0000000..0cda716 --- /dev/null +++ b/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 diff --git a/src/iiwa_utils/iiwa_utils/tcp_gizmo_spawner.py b/src/iiwa_utils/iiwa_utils/tcp_gizmo_spawner.py new file mode 100644 index 0000000..793d5f7 --- /dev/null +++ b/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 "" + 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() diff --git a/src/iiwa_utils/package.xml b/src/iiwa_utils/package.xml index 15a049d..ac9a617 100644 --- a/src/iiwa_utils/package.xml +++ b/src/iiwa_utils/package.xml @@ -12,6 +12,9 @@ ament_index_python webots_ros2_msgs python3-yaml + iiwa_msgs + python3-numpy + python3-scipy ament_copyright ament_flake8 diff --git a/src/iiwa_utils/setup.py b/src/iiwa_utils/setup.py index 2bab6b0..d71e3b8 100644 --- a/src/iiwa_utils/setup.py +++ b/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", ], }, ) diff --git a/src/iiwa_utils/test/test_gizmo_settings.py b/src/iiwa_utils/test/test_gizmo_settings.py new file mode 100644 index 0000000..d0d1e39 --- /dev/null +++ b/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 diff --git a/src/iiwa_utils/test/test_tcp_gizmo.py b/src/iiwa_utils/test/test_tcp_gizmo.py new file mode 100644 index 0000000..0598c61 --- /dev/null +++ b/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)) diff --git a/src/iiwa_utils/test/test_tcp_gizmo_plugin.py b/src/iiwa_utils/test/test_tcp_gizmo_plugin.py new file mode 100644 index 0000000..c6da79b --- /dev/null +++ b/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 == [] diff --git a/src/iiwa_utils/test/test_tool_tcp.py b/src/iiwa_utils/test/test_tool_tcp.py new file mode 100644 index 0000000..823e4f1 --- /dev/null +++ b/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])