Статус: сквозной контур работает в симуляции — товар определяется, классифицируется, захватывается вакуумным захватом и доставляется в целевую зону. Есть Gymnasium-среда для дообучения RL и ROS2-мост для переноса на реальный робот.
Gymnasium-среда SortingPickEnv (src/robozone/rl_env.py). Один эпизод = один
товар из накопителя в целевую зону. Управление 20 Гц (25 подшагов физики на шаг),
до 300 шагов. Целевая зона подаётся средой как условие (one-hot) — RL-часть учит
физическую маршрутизацию, CV-часть подключается независимо.
Кратко о потоке: наблюдение (46) → актёр выдаёт действие (7) →
среда шагает физику и возвращает награду и следующее наблюдение → переход
(s,a,r,s′) кладётся в replay buffer → на выборках из буфера учатся критик
(оценивает качество действий) и актёр (улучшается по оценке критика).
Обёртка над RobozoneSim: на reset спавнит товар с доменной рандомизацией и
выбирает сценарий корнер-кейса (см. ниже); на step интегрирует целевые позиции
суставов в позиционные сервоприводы и обновляет вакуум. Кинематика реальная —
политика переносима на UR10e через ROS2-мост (раздел 7).
| Индексы | Смысл |
|---|---|
0..5 |
приращения целевых позиций 6 суставов, ∈[−1,1] × ACT_SCALE(0.05 рад) |
6 |
вакуум: >0 включить, иначе выключить |
q(6), q̇(6) — состояние руки; ee(3) — TCP присоски; obj_pos(3), obj_quat(4) —
поза товара; ee→obj(3), ee→drop(3); held(1) (реальный захват), suction(1);
zone_onehot(3) — целевая зона; extents(3) — габариты (масштаб товара);
grasp(3), grasp→ee(3) — размерозависимая точка захвата (верх над ЦМ);
contact(1) — тактильный сигнал касания присоски; phase(1) — фаза задачи
(был ли первый контакт, монотонно); recovery(1) — режим восстановления после
срыва; contact_conf(1) — гладкая тактильная уверенность [0,1].
Наивная награда «только по расстоянию» нестабильна для захвата: присоска подлетает → касается → лёгкий товар отталкивается → расстояние снова растёт → RL читает это как ошибку, хотя произошёл первый контакт. Поэтому награда переключает фазу по первому касанию (идея из tactile grasp refinement):
Фаза 1 — подвод (контакта ещё не было): единственный сигнал —
сближение присоски с точкой захвата: Δ(−‖ee − grasp‖).
Первый контакт: разовый бонус +0.5 («нашёл объект»). Дальше расстояние
ee→товар перестаёт влиять — толчок товара больше не штрафуется. Фаза задачи
монотонна и не откатывается назад.
Фаза 2 — манипуляция (после контакта):
- +0.01/шаг за удержание захвата (стабильный контакт, «не теряй товар»);
- перенос ведётся по расстоянию товара (не EE) до точки зоны — оно корректно, т.к. схваченный товар движется вместе с рукой;
- −slip_penalty (2.0) за проскальзывание/срыв (был захват, вакуум включён, товар потерян и не в зоне).
Терминально: +10 успех (товар в целевой зоне, отпущен, покоится),
−5 чужая зона, −3 упал на пол; −0.01/шаг за время. Бонусы за контакт
намеренно малы (не if force>thr: +100) — чтобы агент не «давил» в товар, а
удерживал устойчивый захват.
Проверено: после первого контакта рост
‖ee−obj‖даёт только−0.01(штраф времени), а не большой штраф расстояния (scenario_drop_repick.pyи тесты).
Отдельный тяжёлый случай — когда товар срывается уже после захвата (частый
slip лёгких/скользких деталей, vacuum_weak, резкое движение). Наивные варианты
оба плохи: reset эпизода уничтожает learning signal, а «жёсткий откат фазы в
approach» (наш прежний вариант) ломает причинность — политика воспринимает slip
как перезапуск задачи с нуля, а не как «надо починить захват», и recovery не
выучивается. Как в reset-free / tactile-regrasp / residual-VLA работах, сделано
по формуле regrasp = тот же эпизод + та же фаза + изменённый reward + closed-loop
коррекция:
- фаза задачи не откатывается (
phase/contactedмонотонны); - вместо резкого сброса — гладкая
contact_conf(растёт при контакте, плавно спадает без него) и флагrecovery_mode(оба в наблюдении); - при срыве включается
recovery_modeи reward переключается на восстановление: слабое сведение присоски к товару (realign_weight·Δ(−‖ee−grasp‖)), штраф за скорость товара (−obj_vel_penalty·‖v‖, чтобы не гонять «убегающую» деталь), плюс+regrasp_bonus (0.5)за восстановленный захват; - эпизод завершается только при успехе, чужой зоне или падении на пол (вне досягаемости) — срыв «на столе» остаётся восстановимым в том же эпизоде.
Проверено (
scenario_drop_repick.py+ тесты): срыв → штраф −2.0,contactedостаётсяTrue(фаза не откатилась),recovery_mode=True,contact_confплавно спадает; восстановление захвата →+regrasp_bonus, выход из recovery — всё в пределах одного эпизода.
Чтобы в обучающих данных были неудачные захваты и повторные попытки, каждый
эпизод с заданной вероятностью получает сценарий отказа (SCENARIOS в
rl_env.py). Именно для этих случаев важна контактная награда и re-pick:
| Сценарий | Доля | Что делает | Какой отказ создаёт |
|---|---|---|---|
normal |
20% | без возмущений | — |
shift |
20% | товар смещён на 2–10 мм после осадки | промах/смещение захвата |
rotate |
15% | усиленный наклон/поворот позы | неудобная грань для захвата |
tcp_offset |
15% | смещение TCP/калибровки 2–8 мм в наблюдении | прицел мимо → промах |
friction |
10% | коэффициент трения товара ×[0.4,1.6] | сдвиг/скольжение при касании |
vacuum_delay |
10% | вакуум включается через 3–8 шагов | «касание без захвата» → re-pick |
vacuum_weak |
10% | сила прилипания ~30–110 Н | срыв тяжёлого товара при подъёме |
Проверено: сценарии применяются (трение 0.41, gain 327, задержка 4, bias −8 мм);
vacuum_weak реально срывает тяжёлые товары (box_400, 4 кг) при подъёме,
vacuum_delay требует держать вакуум заранее. SortingPickEnv(inject_scenarios= False) отключает инъекцию; curriculum=True усложняет рандомизацию по успеху.
Алгоритм — SAC (Soft Actor-Critic), off-policy actor-critic для непрерывного управления. Две сети:
- Актёр (политика π) — MLP
[256,256], вход = наблюдение (46), выход = параметры распределения действий: среднееμ(s)иlog_std(s)по 7 осям; действие сэмплируется и сжимаетсяtanhв[−1,1]= наше действие (7) (6 приращений суставов + вакуум). Стохастичность = встроенное исследование; на инференсе берётся детерминированноtanh(μ(s)). - Критик (две Q-сети
Q₁,Q₂(s,a)) — MLP[256,256], вход = наблюдение + действие, выход = скалярная оценка ожидаемой суммарной будущей награды при выбореaв состоянииs. Именно критик «понимает», что подвод→захват→укладка ценны, а «ничего не делать» — нет.
Как учится критик. По переходам (s,a,r,s′) из replay buffer минимизируется
TD-ошибка к цели Беллмана:
y = r + γ·( minᵢ Q_targetᵢ(s′,a′) − α·log π(a′|s′) ), где a′∼π(·|s′),
γ=0.98. Два критика и min борются с переоценкой Q; target-сети
обновляются мягко (τ=0.02) для устойчивости; член −α·log π («soft») поощряет
не схлопывать политику. Актёр обновляется в обратную сторону — максимизирует
minᵢ Qᵢ(s, a∼π) + α·H(π) (действие репараметризовано, градиент течёт через
критика в актёра).
Почему off-policy важно. Критик и актёр учатся на случайных выборках из
буфера, а не только на свежих эпизодах — поэтому в буфер можно подложить
демонстрации эксперта, и критик сразу видит успешные (s,a,r,s′).
Чистый SAC «с нуля» на этой задаче не обучается — застревает в оптимуме
«ничего не делать»: координированную последовательность подвод→захват→укладка
почти невозможно нащупать случайным исследованием, критик не видит ни одного
успеха, и reward держится на уровне штрафа за время (ep_len≈300, timeout).
Рабочий путь — warm-start демонстрациями (reset-free / SAC-from-Demonstrations):
- Скриптовый эксперт (
src/robozone/expert.py) — конечный автомат на IK, выдаёт действия в формате среды (~74% успеха). - Сбор демонстраций (
collect_demos.py) — переходы(s,a,r,s′)успешных эпизодов (≈30.6k). - Behavior Cloning актёра (
train_bc_sac.py) — супервайзно обучаем актёра воспроизводить действия эксперта:MSE(tanh(μ(s)), a_expert). Ключевой нюанс: измерение вакуума бинарное — при обычном MSE усредняется к «выкл» и политика не захватывает; поэтому оно взвешено ×5. BC в одиночку даёт ~72–77%. - Seed replay buffer — кладём демонстрации в буфер, чтобы критик сразу учился на успешных переходах, а не с нуля.
- Мягкое дообучение SAC поверх: низкий
log_std(−2) и малыйent_coef(0.02, вместо «auto»),lr=1e-4— иначе стохастичность и Q-максимизация SAC разрушают точную BC-политику.
python scripts/collect_demos.py --episodes 300 --out runs/demos.npz
python scripts/train_bc_sac.py --demos runs/demos.npz --bc-epochs 60 \
--timesteps 250000 --ent-coef 0.02 --init-log-std -2.0 --outdir runs/sac_bc
python scripts/eval_rl.py --policy runs/sac_bc/final.zip --algo sacРезультат обученной политики (runs/sac_bc/final.zip, deterministic):
76% без сценариев отказов (B 100%, D 85%, C 25%) и 61% со сценариями
(B 100%, D 57%, C 46%). Зона C (крупногабарит/тонкие) — ожидаемо самая сложная.
Честный вывод по online-дообучению: последующий online-SAC поверх BC ухудшает политику (актёр уходит от демонстраций к действиям с высоким Q при ещё не откалиброванном критике) — даже с малым
ent_coef, низкимlog_stdи BC-регуляризацией (--bc-coef). Это известная сложность offline→online RL. Решение реализовано в AWAC (раздел ниже) — advantage-weighted actor-critic, гарантирующий что актёр остаётся на данных. Итоговая BC политика — 76%/61%.
AWAC (Advantage-Weighted Actor-Critic) решает проблему SAC на offline→online задачах. Вместо Q-максимизации (которая уводит актёра далеко от данных), AWAC обновляет актёра как взвешенную регрессию к действиям из буфера:
L_π = − E_{s,a~buffer} [ log π(a|s) · exp( A(s,a) / λ ) ]
A(s,a) = Q(s,a) − V(s)
Веса exp(A/λ) усиливают действия с высоким преимуществом, но актёр остаётся близко
к распределению демонстраций (гарантирует стабильность при плохой калибровке критика).
Пайплайн AWAC (runs/awac_v2):
5k BC инициализация → актёр ≈72% (как в train_bc_sac.py)
↓
30k offline AWAC → критик обучается на демо буфере, актёр уточняется
↓
50k online AWAC → взаимодействие среды со сценариями отказов, learned recovery
↓
Результат: ожидание 75–80% clean, 70+ scenarios (улучшение на recovery-сценариях)
Особенности AWAC:
- Хорошо работает на recovery сценариях (vacuum_weak, vacuum_delay) — online видит отказы, учится восстанавливаться
- Улучшает переадаптацию (shift, rotate, tcp_offset) — мелкие вариации в BC приносят преимущество
- Гарантирует не деградировать относительно BC (advantage-weighted регуляризация)
Запуск и оценка:
python scripts/train_awac.py --demos runs/demos.npz \
--offline-steps 30000 --online-steps 50000 --out runs/awac_v2
# Сравнительный отчёт на 8 сценариях (normal, shift, rotate, tcp_offset, friction, vacuum_delay, vacuum_weak):
python scripts/final_awac_report.py \
--bc-policy runs/sac_bc/final.zip \
--awac-policy runs/awac_v2/final.pt \
--n-episodes 50pip install -r requirements-rl.txt # SB3 (+torch)
python scripts/train_rl.py --algo sac --timesteps 300000 --n-envs 4
python scripts/eval_rl.py --policy runs/sac_bc/final.zip --algo sac # оценка + видео
python scripts/scenario_drop_repick.py # сценарий срыва детали
python scripts/record_regrasp.py --out demo_pick.mp4 # видео regrasp (POV манипулятора)
demo_pick.mp4— видео сценария regrasp от лица манипулятора (камера следует за захватом): подвод → захват → срыв → recovery → повторный захват → укладка, с онлайн-подписями фаз, флаговконтакт/захват/recoveryиcontact_conf.
| Часть | Где | Что делает |
|---|---|---|
| Определение + классификация | src/robozone/classification.py |
габариты по OBB + признак «круг в сечении» (K=0.8) → категория/зона |
| Сцена симуляции | sim/scene.xml, sim/cell_ur10e.xml, sim/objects.xml |
конвейер A, накопитель, зоны B/C/D, манипулятор UR10e + вакуумный захват |
| Ядро симуляции | src/robozone/sim_core.py |
ленты, спавн товаров, вакуум, детекция зон |
| Исполнительная логика (IK) | src/robozone/ik.py, scripts/demo_pick.py |
скриптовая политика захвата и укладки |
| RL-среда | src/robozone/rl_env.py |
Gymnasium-среда pick-and-route для дообучения |
| ROS2-мост | ros2_ws/src/robozone_sim/ |
узлы sim_node, classifier_node, orchestrator_node |
| Конвертация моделей | scripts/convert_step.py |
STEP → STL (тестовый набор организаторов) |
Соответствие осям УГТ: ось «определение/классификация» — раздел 4; ось «исполнительная часть» — разделы 5–6; связка — раздел 7.
# 0) окружение (Python 3.11). В этой машине готовый venv:
source ~/ros2_kilted/.venv/bin/activate
# зависимости (если ставить с нуля):
pip install mujoco gymnasium trimesh cascadio shapely scipy networkx rtree \
pillow imageio imageio-ffmpeg
cd <репозиторий>
export PYTHONPATH=src
# 1) (однократно) конвертация STEP-моделей организаторов -> STL
python scripts/convert_step.py
python scripts/build_objects_xml.py # sim/objects.xml из STL
# 2) классификация всего тестового набора (пишет categories.json)
python -m robozone.classification
# 3) сквозная демонстрация контура (скриптовая политика) + видео
python scripts/demo_pick.py --mode accumulator --video demo_route.mp4
# 4) видео сценария regrasp от лица манипулятора (demo_pick.mp4)
python scripts/record_regrasp.py --out demo_pick.mp4
# 5) проверка RL-среды (без обучения)
python scripts/eval_rl.py --policy random --episodes 12Просмотр сцены интерактивно: python -m mujoco.viewer --mjcf=sim/scene.xml.
Мировая система координат: X — направление подачи (точка A → накопитель), Y — влево (в сторону зоны B), Z — вверх. Начало — центр накопителя. Рабочая зона 6000×10000 мм воспроизведена по схеме организаторов.
[B] инфид сортировщика (фиксирован)
▲ питающая лента (диверторная)
A ─ подающий конвейер 1 м/с ─▶ ╔═════╗ │
(лента 500×700) ║накоп║──┤ UR10e + вакуумный захват
╚═════╝ │
▼ ▼
[C] ролл-кейдж [D] ролл-кейдж
негабарит доупаковка
| Точка | Назначение | Реализация в сцене |
|---|---|---|
| A | подача товара | лента 500 мм, верх 700 мм, скорость 1 м/с, заканчивается накопителем |
| B | основной сортировщик | питающая лента → инфид сортера (положение фиксировано) |
| C | негабарит | ролл-кейдж 1200×800×800 |
| D | доупаковка | ролл-кейдж 1200×800×800 |
Ленты моделируются физически: тело ленты на слайд-шарнире движется velocity- актуатором, координата периодически «сматывается» — поверхность однородна, поэтому товары переносятся честным трением (бесконечная лента).
Категория определяется строго по правилам постановки (приоритет габаритов над формой):
- Габариты по минимальному ориентированному параллелепипеду (OBB) —
инвариантно к позе товара на ленте. Если не в диапазоне
(10×10×10 … 450×320×320)мм → C «Не подходит по габаритам». - Круг в сечении: для набора сечений вдоль трёх главных осей OBB считается
K = r_впис / R_опис(внешняя огибающая сечения). Еслиmax K > 0.8→ D «Не подходит без доупаковки». - Иначе → B «Подходит для сортировки».
Реализация: src/robozone/classification.py (trimesh + shapely). Результат по
тестовому набору организаторов (assets/meshes/objects/categories.json):
| Товар | Зона | K (макс. r/R) | OBB, м | Категория |
|---|---|---|---|---|
| box_300x200x200 | B | 0.720 | 0.301 × 0.201 × 0.200 | подходит |
| detergent (моющее средство) | B | 0.742 | 0.278 × 0.260 × 0.180 | подходит |
| lunchbox (ланчбокс) | B | 0.646 | 0.201 × 0.152 × 0.062 | подходит |
| box_400x400x300 | C | 0.716 | 0.401 × 0.400 × 0.301 | негабарит (>320 мм) |
| pen (ручка) | C | 0.842 | 0.149 × 0.013 × 0.009 | негабарит (<10 мм по толщине) |
| pouf (пуфик) | C | 0.994 | 0.489 × 0.489 × 0.264 | негабарит (>450 мм) |
| bag (мешок) | D | 0.896 | 0.202 × 0.175 × 0.170 | круг в сечении |
| bottle (бутылка) | D | 0.995 | 0.305 × 0.091 × 0.091 | круг в сечении |
| cylinder (цилиндр) | D | 0.867 | 0.435 × 0.050 × 0.043 | круг в сечении |
| helmet (шлем) | D | 0.895 | 0.354 × 0.297 × 0.280 | круг в сечении |
| plate (тарелка) | D | 0.998 | 0.209 × 0.209 × 0.027 | круг в сечении |
Приоритет соблюдён: box_400x400x300 и pouf имеют высокий K, но из-за
габаритов отнесены к C (проверка габаритов выполняется первой). pen
проходит по форме, но толщина 9 мм < 10 мм → C.
В симуляции категория подаётся исполнительной части как «выход CV-части» (ground truth из
categories.json). Это позволяет отлаживать исполнительный контур независимо от зрения; при переносе на реальную систему узелclassifier_nodeзаменяется на инференс CV-модели по кадру камеры с тем же выходным контрактом (строка зоны B/C/D) — остальной контур не меняется. В сцене есть калиброванные камерыcam_overhead,cam_pick,cam_wrist.
Манипулятор: UR10e (модель из MuJoCo Menagerie, BSD-3), 6 степеней свободы,
имена суставов сохранены как у реального робота (shoulder_pan_joint … ) —
критично для переноса. На фланец добавлен вакуумный захват: присоска +
adhesion-актуатор suction (эмуляция вакуума, сила прилипания на контактах
присоски). Выбор вакуумного захвата обоснован разнородностью потока (коробки,
цилиндры, шлемы, тарелки) и требованием мягкого обращения — см. справочный раздел
постановки.
Ключевые узлы:
- подающий конвейер A (1 м/с) → накопитель (роликовый стол с бортами);
- роботизированная ячейка UR10e на тумбе (верх на уровне ленты 700 мм);
- питающая (диверторная) лента к инфиду сортировщика (зона B за пределами вылета руки — товар кладётся на ближний конец ленты, лента переносит к точке B);
- ролл-кейджи C и D — открытые сверху, товар сбрасывается манипулятором.
Логика захвата и укладки (ik.py, sim_core.py):
- точка присасывания — реальная верхняя грань товара в текущей позе (луч сверху вниз), а не по габаритам меша: товары ложатся плашмя (напр. ручка);
- дифференциальная IK (damped least squares) по сайту присоски, ориентация оси захвата — вниз;
- смещение захвата измеряется в момент прилипания и учитывается при укладке
(
placement_tip_target): B — укладка на ленту, C/D — сброс сверху в кейдж.
python scripts/demo_pick.py --mode accumulator --video demo_pick.mp4
# один товар едет по ленте A -> накопитель:
python scripts/demo_pick.py --objects bottle --mode beltСкриптовая политика для каждого товара: спавн → (доезд по ленте) → классификация → IK-захват присоской → перенос → укладка в зону B/C/D → возврат в домашнюю позу. Траектория укладки: подъём → перенос высоко над зоной → вертикальный спуск (устойчиво к застреваниям жадной IK).
Результат базовой скриптовой политики: 9/11, все три зоны
(B: 3/3, D: 5/5, C: 1/3). Не удаются pen (9 мм — тонкая деталь для захвата
сверху) и pouf (489 мм, мягкий крупногабарит) — задокументированные
пограничные случаи. Подробности — REPORT.md.
Мост построен на стандартных сообщениях ROS2 (без custom-msg — проще проверять и разворачивать). Три узла:
| Узел | Публикует | Подписан на | Сервисы |
|---|---|---|---|
sim_node |
joint_states, ee_pose, suction_state, active_object, object_pose, TF |
position_command, suction_command, spawn |
reset, set_suction |
classifier_node |
category, target_zone |
active_object |
— |
orchestrator_node |
position_command, suction_command |
joint_states, object_pose, suction_state, target_zone |
— |
Топик position_command (Float64MultiArray, 6 целевых позиций суставов)
совместим по смыслу с forward_position_controller из ur_robot_driver —
замена симуляции на реального робота не меняет остальной контур.
Оркестратор считает кинематику на read-only копии модели (FK/якобиан) —
на реальном роботе заменяется на URDF+KDL с тем же контрактом топиков.
Запуск полного контура:
# один раз: собрать ROS2-пакет
cd ros2_ws && colcon build --packages-select robozone_sim && source install/setup.zsh
export ROBOZONE_ROOT=<репозиторий> # где лежит sim/scene.xml
ros2 launch robozone_sim sim_bringup.launch.py spawn_mode:=accumulator
# ручное управление без скриптовой политики (для RL):
ros2 launch robozone_sim sim_bringup.launch.py run_orchestrator:=falseЛента A движется 1 м/с. CV-часть работает с опережением (товар классифицируется в накопителе до захвата), а положение товара к моменту захвата определяется по кадру камеры/позе — задержка инференса не смещает захват. Неоднозначные случаи (низкая уверенность) в реальной системе направляются в отдельный поток на повторный прогон.
Требования: Linux/macOS, Python 3.11, ROS2 (проверено на Kilted), пакеты из раздела 2. GPU не обязателен для проверки контура (нужен для обучения).
Параметры, которыми управляет эксперт (через launch/сервисы/топики):
spawn_mode=accumulator|belt;belt_speed(м/с);/robozone/spawn(String) — подать конкретный товар илиrandom;/robozone/reset— сброс сцены;/robozone/set_suction— вакуум;run_orchestrator:=false— отключить скриптовую политику для ручного/RL-управления.
Сценарии проверки: (1) классификация всего набора python -m robozone.classification; (2) сквозная укладка scripts/demo_pick.py;
(3) RL-среда scripts/eval_rl.py; (4) ROS2-контур через launch.
- Тонкие товары (ручка 9 мм) — вакуумный захват сверху ненадёжен; в реальной системе — щелевой/боковой захват или отдельный поток. Это пограничный случай, зафиксирован в проверках.
- Классификация в симуляции использует ground-truth категории; CV-инференс по камере — следующий шаг (контракт узла уже задан).
- Скриптовая политика — опорная; RL-политика обучается той же средой.
- Захват предполагает доступную верхнюю грань; плотная упаковка в накопителе не моделируется.
MuJoCo 3.10, Gymnasium, ROS2 Kilted (rclpy), UR10e из MuJoCo Menagerie
(BSD-3), trimesh + cascadio (STEP→STL), shapely (сечения), NumPy/SciPy,
Stable-Baselines3 (RL), imageio (видео). Полный список версий — в REPORT.md.
sim/ сцены MuJoCo (scene, cell_ur10e, objects)
src/robozone/ classification, sim_core, ik, rl_env
scripts/ convert_step, build_objects_xml, demo_pick, train_rl, eval_rl
ros2_ws/src/robozone_sim/ ROS2-узлы, launch
assets/ меши UR10e и тестовых товаров, categories.json
models_files/ исходные STEP-модели организаторов