feat(experement): plan transfers with Pilz PTP instead of OMPL

- _contact transfer/approach and _retreat clear now use deterministic Pilz
  PTP joint interpolation; OMPL remains only the automatic fallback
- move_to_joints on the motion server tries PTP first, OMPL on failure
- verified in Webots execute mode: 1, 11, 14, 20 completed; no PTP failures
  in 1/11/14/20 server logs, OMPL used only as the fallback

src/iiwa_experement/doc/reference-control-qa.md

@@ -87,10 +87,16 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
   горизонтального перемещения. Захваченные инструмент, люк и винт
   прикрепляются к роботу в сцене MoveIt (`bridge.attach`), поэтому перенос
   планируется с учётом переносимого тела.
+- Трансферы и подходы планируются Pilz PTP — детерминированная суставная
+  интерполяция вместо случайных дуг OMPL RRTConnect. Короткие осевые
+  участки — Pilz LIN. OMPL остаётся только автоматическим запасным
+  вариантом, если PTP не нашёл решение. Суставные цели (`move_to_joints`:
+  переходы в `work`/`home`, поворот фланца) также идут через PTP с
+  откатом на OMPL при отказе.
 - TCP при работе направлен вниз; затяжной инструмент вращается вокруг своей
-  оси, не меняя точку контакта. После поворота фланец возвращается в
+  оси, не меняя точку контакта. После поворота выполняется подъём, и только
-  исходную ориентацию (untwist), чтобы сустав 7 не накапливал закрутку
+  затем фланец возвращается в исходную ориентацию (untwist): раскрутка на
-  между отверстиями.
+  месте отверстия визуально откручивала бы имитацию.
 - Положение после суставного перемещения проверяется по установившимся
   показаниям `/joint_states`, а не по первому отсчёту после ответа контроллера.
   При кратковременном отклонении цель повторяется один раз.

src/iiwa_experement/iiwa_experement/operations.py

@@ -317,10 +317,13 @@ class ReferenceOperations:
         self.motion.wait_stationary()
         transfer = self._above(goal, transfer_gap)
         approach = self._above(goal, approach_gap)
-        self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:transfer')
+        # Transfers and approaches use Pilz PTP: deterministic joint-space
+        # interpolation instead of OMPL's random wide elbow sweeps.  OMPL
+        # remains the automatic fallback of _move when PTP cannot plan.
+        self._move(transfer, 'ptp', self.geometry.speeds['transfer'], f'{label}:transfer')
         try:
             self._move(
-                approach, 'ompl', self.geometry.speeds['approach'],
+                approach, 'ptp', self.geometry.speeds['approach'],
                 f'{label}:approach',
             )
         except OperationError as error:
@@ -367,7 +370,7 @@ class ReferenceOperations:
                 f'{label}: the axial retreat was rejected ({last_error})'
             )
         self._move(
-            self._above(goal, self.geometry.transfer_gap), 'ompl',
+            self._above(goal, self.geometry.transfer_gap), 'ptp',
             self.geometry.speeds['transfer'], f'{label}:clear',
         )
 

src/iiwa_experement/test/test_operations.py

@@ -155,8 +155,8 @@ def test_take_tool_follows_the_visible_axial_route():
     assert bridge.attached == ['grant_vacuum']
     contact_goal = motion.moves[2][3]
     assert contact_goal.xyz == pytest.approx(expected.xyz, abs=1e-9)
-    assert motion.moves[0][1] == 'ompl'
+    assert motion.moves[0][1] == 'ptp'
-    assert motion.moves[1][1] == 'ompl'
+    assert motion.moves[1][1] == 'ptp'
     assert motion.moves[2][1] == 'lin'
 
 

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -298,10 +298,22 @@ class IiwaMotionServer(Node):
         feedback.state = "executing"
         goal_handle.publish_feedback(feedback)
 
+        # Суставные цели — классический PTP (детерминированная интерполяция
+        # без случайных дуг RRTConnect); OMPL — запасной вариант.
         plan_params = self._make_plan_params(
-            "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
+            "pilz_industrial_motion_planner", "PTP", 2.0, velocity_scale
         )
         ok, msg = self._plan_and_execute(plan_params, goal_handle)
+        if ok is False:
+            self.get_logger().warn(
+                f"PTP не справился ({msg}); переключаюсь на OMPL"
+            )
+            self._arm.set_start_state_to_current_state()
+            self._arm.set_goal_state(robot_state=goal_state)
+            plan_params = self._make_plan_params(
+                "ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
+            )
+            ok, msg = self._plan_and_execute(plan_params, goal_handle)
         return result, ok, msg
 
     def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):