diff --git a/src/iiwa_experement/doc/reference-control-qa.md b/src/iiwa_experement/doc/reference-control-qa.md index ab87147..0e152ed 100644 --- a/src/iiwa_experement/doc/reference-control-qa.md +++ b/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`, а не по первому отсчёту после ответа контроллера. При кратковременном отклонении цель повторяется один раз. diff --git a/src/iiwa_experement/iiwa_experement/operations.py b/src/iiwa_experement/iiwa_experement/operations.py index e330dab..372afa4 100644 --- a/src/iiwa_experement/iiwa_experement/operations.py +++ b/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', ) diff --git a/src/iiwa_experement/test/test_operations.py b/src/iiwa_experement/test/test_operations.py index 6a5bee0..218f29e 100644 --- a/src/iiwa_experement/test/test_operations.py +++ b/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[1][1] == 'ompl' + assert motion.moves[0][1] == 'ptp' + assert motion.moves[1][1] == 'ptp' assert motion.moves[2][1] == 'lin' diff --git a/src/iiwa_planning/scripts/move_to_pose_server.py b/src/iiwa_planning/scripts/move_to_pose_server.py index ff805a1..2065cf1 100644 --- a/src/iiwa_planning/scripts/move_to_pose_server.py +++ b/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):