feat(control): unify joint limits and bound trajectory dynamics

Use shared limits for all seven axes across MoveIt, URDF and FRI.
Apply Ruckig smoothing and continuous JTC spline checks, stretching
trajectory timing to respect velocity, acceleration and jerk limits.
Require fresh stationary feedback for direct trajectories and protect
preparation from concurrent submissions and stop requests.

Add regression tests, dependencies and bilingual FRI documentation.
Include the updated LIN speed-sweep scenario and current 60 ms filter
configuration. Acceleration and jerk limits remain project settings;
physical high-speed twitching is still under investigation.
This commit is contained in:
Даниил Грабарь
2026-09-21 05:16:09 +03:00
parent 9f0f05df87
commit 0a091ad521
43 changed files with 1342 additions and 137 deletions
+29
View File
@@ -82,3 +82,32 @@ sudo gpasswd -d "$USER" realtime
После повторного входа процессы снова будут работать с обычным планировщиком Linux.
Подробнее о лимитах и приоритете `controller_manager` — в [документации ros2_control](https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html#determinism).
## Ограничения траекторий и плавность
`controller.moveit.joint_limits` из `cobot-setting.yaml` задаёт единый файл для всех семи осей. Его читают URDF/xacro, FRI, Webots, MoveIt и web API. `min_position/max_position` описывают рабочие жёсткие пределы; `soft_min_position/soft_max_position` сохраняют более узкие защитные диапазоны. Планирование, командные интерфейсы и проверка API используют пересечение этих диапазонов. При загрузке MoveIt пересечение применяется явно: YAML иначе может переопределить мягкие пределы URDF.
Диапазоны и скорости сверены с [листом KUKA LBR iiwa 7 R800, 0000-246-832 V1.2](https://www.kuka.com/-/media/kuka-downloads/imported/8350ff3ca11642998dbdc81dcc2ed44c/0000246832_en.pdf). Для осей A1…A7 паспортные диапазоны составляют ±170°, ±120°, ±170°, ±120°, ±170°, ±120°, ±175°; скорости с номинальной нагрузкой — 98, 98, 100, 130, 140, 180, 180°/с. Рабочие пределы не расширены относительно прежнего URDF: округления, превышавшие паспортные значения, уменьшены. **Ускорения и рывки в YAML остаются ограничениями проекта, а не подтверждёнными паспортными пределами KUKA.** Данные установленного инструмента, нагрузки и настройки Sunrise по этому листу не проверяются.
Для OMPL/CHOMP обработка выполняется в порядке:
1. TOTG назначает время движения.
2. `iiwa_planning/SetRestBoundary` проверяет скорость исходного состояния и задаёт нулевые скорости/ускорения на концах движения из покоя.
3. Ruckig сглаживает траекторию с ограничением рывка.
4. Повторная установка граничных условий устраняет изменения производных, которые может внести продление длительности в Ruckig.
5. `iiwa_planning/LimitSplineDynamics` проверяет весь квинтический сплайн JTC, включая промежутки между точками. Границы полиномов вычисляются в базисе Бернштейна, а не только по временной сетке. При превышении скорости, ускорения или рывка увеличивается время всей траектории; скорости и ускорения масштабируются согласованно. Выход позиции за рабочие пределы отклоняется.
6. MoveIt проверяет решение на столкновения обычным адаптером `ValidateSolution`.
Pilz PTP/LIN/CIRC проходят проверку сплайна и равномерное увеличение длительности без Ruckig в суставном пространстве. При этом сохраняется исходная кривая интерполяции JTC; точность геометрии LIN/CIRC относительно непрерывного идеального пути отдельно не улучшается. Движения с ненулевыми граничными скоростями/ускорениями (включая некоторые варианты blending) отклоняются: проверка предназначена для перемещений из покоя в покой. `planning.stopped_velocity_tolerance` задаёт порог определения неподвижного начального состояния в рад/с.
CSV/JSON `/trajectory/send*` по-прежнему принимают позиции и время, но теперь строят гладкий сплайн с нулевыми граничными производными и проходят тот же C++-валидатор. Промежуточные точки сохраняются, однако кривая между ними может отличаться от прежней линейной интерполяции. Прямой API не выполняет проверку столкновений. Ответ возвращает фактические `duration` и `time_scale`; времена могут увеличиться. `validate_limits=false` не отключает обязательную проверку динамики и рабочего диапазона.
Перед отправкой требуется свежая обратная связь с именами осей, положениями и скоростями; робот должен быть неподвижен. Точка при `t=0` должна совпадать с текущим положением в пределах `web.trajectory_start_tolerance`. Если первая точка позже нуля, текущее положение добавляется в начало. `web.trajectory_state_timeout` ограничивает возраст состояния; `web.trajectory_stopped_velocity` — начальную скорость. Состояние повторно проверяется после расчёта. Повторная прямая отправка во время активной траектории отклоняется; остановка отменяет ещё не завершённую подготовку.
Внешний ROS-клиент, публикующий прямо в топик JTC или отправляющий ему action, **обходит подготовку MoveIt/web**. Он обязан самостоятельно формировать корректные траектории; это изменение не является общей защитой от произвольных ROS-команд. FRI-фильтр, watchdog и период обмена не менялись. Повторное аппаратное испытание необходимо для оценки шума и фактического слежения.
### Проверка без оборудования
После сборки можно выполнить `colcon test --packages-select iiwa_planning iiwa_utils iiwa_web iiwa_controller --python-testing pytest`. Тесты проверяют согласованность семи осей в реальном и симуляционном URDF, пики между точками, результат установленного интерполятора JTC, связку установленного TOTG/Ruckig с pluginlib и отклонение устаревшего состояния web. Они не подключаются к реальному FRI-сеансу. Для нового кода требуется пересборка перечисленных пакетов и `iiwa_config`, `iiwa_description`, `iiwa_bringup`; старый запущенный процесс автоматически не обновляется.
Проверка покоя в адаптерах подписывается на исходный `/joint_states`: отсутствие скоростей в `RobotState` MoveIt не трактуется как нулевая скорость. `planning.state_timeout` ограничивает возраст исходной телеметрии. Тест подписки использует отдельный локальный ROS domain 231 и синтетические сообщения, без FRI.