fix(experement): harden tool transfers and tightening at wrist limits

Exit the rack with a graded +Y move after taking or docking a tool, continue through home after a dock, and fall back from work to home when a joint plan is rejected. Wait for fresh zero-velocity joint states before planning and after joint motion, restore a screw to the planning scene before a pick retry, and twist only the flange joint while simulating tightening so the arm does not leave a reachable IK branch.
This commit is contained in:
Даниил Грабарь
2026-11-09 01:29:36 +10:00
parent a6c6474db9
commit 41144ecd2a
6 changed files with 215 additions and 70 deletions
@@ -61,9 +61,10 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
## Политика движений
- После захвата инструмент поднимается вертикально на 16 см, и только затем
выполняется боковое или суставное перемещение. Так корпус инструмента не
проходит сквозь текстуры стойки.
- После захвата инструмент поднимается на 8 см, затем смещается на 10 см по
+Y от стойки, и только потом выполняется суставное перемещение. Полный
вертикальный подъём на 14–16 см упирается в предел запястья; боковой выход
даёт тот же зазор и не ломает достижимость `work`.
- Между операциями используется ближняя нейтральная поза `work`. `home`
остаётся финальной позой маршрута, позой перед установкой инструмента в
стойку и аварийной реконфигурацией.
@@ -139,9 +140,11 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
В опытах 14, 15 и 16–20 после передач винтов берётся `torque_tool`. Для
каждого отверстия выполняется `tighten_simulated`: подъём над отверстием,
осевое опускание, поворот на 60°, возврат и осевой отход. Если у дальнего
отверстия запястье упирается в предел, поворот выполняется в противоположную
сторону. Отверстие остаётся пустым; винт не вставляется. Инструмент
осевое опускание и поворот последнего сустава на 45°. Вращается только
фланец, поэтому инструмент поворачивается вокруг своей оси, не уходя в другую
ветвь IK. Если положительное направление упирается в предел, используется
отрицательное. После поворота робот уходит в `work`, как после передачи
винта. Отверстие остаётся пустым; винт не вставляется. Инструмент
возвращается в стойку, затем робот уходит домой.
## Ветви отказов
@@ -162,7 +165,7 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
- Только симуляция Webots; FRI и физический робот не запускаются.
- Винты не вставляются; после передачи отверстие остаётся пустым.
- Затяжка имитационная: `torque_tool` подходит к отверстию, поворачивается на
60° вокруг оси инструмента и отходит. Винт к этому моменту уже передан
45° вокруг оси инструмента и отходит. Винт к этому моменту уже передан
оператору и удалён из сцены.
- Прикреплённые объекты MoveItPy в установленной версии приводят к ложным
столкновениям. Переносимый объект ведёт supervisor; статическая сцена,