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

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

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

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

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

Validation: 42 Python tests and 8 C++ tests passed. Webots regressions covered
one action per release, translation and rotation with and without a tool,
unreachable targets, and ordinary commands waiting for gizmo completion.

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(),
         ))
 

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)]

src/iiwa_bringup/launch/supported/tcp_gizmo_nodes.py

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

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

@@ -22,6 +22,7 @@ def _spawn_setup(context, *args, **kwargs):
     world = LaunchConfiguration("world").perform(context)
     controller = LaunchConfiguration("controller").perform(context)
     initial_positions_file = LaunchConfiguration("initial_positions_file").perform(context)
+    gizmo_enabled = LaunchConfiguration("tcp_gizmo_enabled", default="false").perform(context) == "true"
 
     robot_description = converter.load_robot_description(
         model_path=description,
@@ -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)

src/iiwa_bringup/package.xml

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

src/iiwa_config/config/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

src/iiwa_config/config/tools.yaml

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

src/iiwa_description/urdf/tools/patron.xacro

@@ -107,7 +107,8 @@
     </joint>
 
     <joint name="patron_tcp" type="fixed">
-        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5708"/>
+        <!-- Working end of patron.dae, 55.2 mm from the patron link origin. -->
+        <origin xyz="0.0 0.0 0.0552" rpy="0.0 0.0 -1.5708"/>
         <parent link="patron"/>
         <child link="tcp"/>
     </joint>
@@ -174,4 +175,4 @@
         </sensor>
     </gazebo> 
 
-</robot>
+</robot>

src/iiwa_msgs/CMakeLists.txt

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

src/iiwa_msgs/msg/GizmoCommand.msg

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

src/iiwa_msgs/msg/GizmoState.msg

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

src/iiwa_msgs/srv/MotionPriority.srv

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

src/iiwa_planning/CMakeLists.txt

@@ -9,6 +9,8 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_python REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(control_msgs REQUIRED)
 find_package(rclpy REQUIRED)
 find_package(moveit_ros_planning_interface REQUIRED)
 find_package(moveit_core REQUIRED)
@@ -16,6 +18,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()

src/iiwa_planning/iiwa_planning/motion_priority.py

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

src/iiwa_planning/include/iiwa_planning/tcp_gizmo_policy.hpp

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

src/iiwa_planning/package.xml

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

src/iiwa_planning/scripts/motion_sequence_runner.py

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

src/iiwa_planning/scripts/move_to_pose_server.py

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

src/iiwa_planning/src/tcp_gizmo.cpp

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

src/iiwa_planning/test/gizmo_webots_regression.py

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

src/iiwa_planning/test/gizmo_webots_speed.py

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

src/iiwa_planning/test/test_motion_priority.py

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

src/iiwa_planning/test/test_tcp_gizmo_policy.cpp

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

src/iiwa_utils/iiwa_utils/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")

src/iiwa_utils/iiwa_utils/tcp_gizmo.py

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

src/iiwa_utils/iiwa_utils/tcp_gizmo_model.py

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

src/iiwa_utils/iiwa_utils/tcp_gizmo_spawner.py

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

src/iiwa_utils/package.xml

@@ -12,6 +12,9 @@
   <depend>ament_index_python</depend>
   <depend>webots_ros2_msgs</depend>
   <depend>python3-yaml</depend>
+  <depend>iiwa_msgs</depend>
+  <depend>python3-numpy</depend>
+  <depend>python3-scipy</depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/iiwa_utils/setup.py

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

src/iiwa_utils/test/test_gizmo_settings.py

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

src/iiwa_utils/test/test_tcp_gizmo.py

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

src/iiwa_utils/test/test_tcp_gizmo_plugin.py

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

src/iiwa_utils/test/test_tool_tcp.py

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