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