UR5 Obstacle-Avoidance Reaching — Sim (MuJoCo) → Replay (ur_rtde)

Perencanaan jalur ruang-sendi bebas-tabrakan untuk lengan UR5: rencanakan di simulasi MuJoCo, hindari obstacle, lalu replay sudut sendi yang sama ke UR5 fisik via ur_rtde. Tanpa ROS/MoveIt, tanpa torque/RL, tanpa gripper.

⚠️ Ini OPEN-LOOP. Baca ini dulu.

Robot tidak menginderai lingkungan saat eksekusi. Ia hanya memutar ulang sudut sendi yang dihitung offline. Jaminan "nol tabrakan" hanya berlaku jika dunia fisik persis sama dengan angka di config.yaml (obstacle, platform, base) dan posisi start nyata sesuai rencana. Kalau ada yang bergeser, robot bisa menabrak. Selalu dry-run pelan dengan tangan di E-STOP.

---

1. Prinsip: satu sumber kebenaran

Semua geometri & parameter ada hanya di config.yaml, dibaca oleh sim (plan.py) maupun eksekusi (execute_real.py). Tidak ada angka yang di-hardcode di kode. Sim dan robot asli memakai file yang sama → konsisten.

2. Struktur project

simulasi/
├─ config.yaml            ← SATU SUMBER KEBENARAN (edit di sini saja)
├─ plan.py                ← jalankan planning di SIM, export trajektori
├─ run_viewer.py          ← visualisasi (butuh display)
├─ execute_real.py        ← replay ke UR fisik via ur_rtde (manual, hati-hati)
├─ requirements.txt
├─ mujoco_menagerie/
│  └─ universal_robots_ur5e/   ← model UR5e (sudah disertakan)
├─ ur5planner/            ← paket inti
│  ├─ config_loader.py    ← baca & validasi config
│  ├─ build_scene.py      ← rakit scene MuJoCo (robot+platform+obstacle) via MjSpec
│  ├─ ik.py               ← IK Jacobian damped-least-squares (mj_jac)
│  ├─ collision.py        ← collision-check robust via mj_geomDistance
│  ├─ rrt_connect.py      ← planner RRT-Connect ruang sendi + benchmark
│  ├─ smoothing.py        ← shortcut smoothing
│  ├─ time_param.py       ← profil trapesium (hormati limit vel/accel)
│  ├─ validate.py         ← cek nol-tabrakan + limit, buat laporan
│  ├─ pipeline.py         ← orkestrasi (dipakai plan & execute)
│  └─ viewer.py           ← playback MuJoCo
└─ generated/             ← OUTPUT (dibuat otomatis)
   ├─ scene_generated.xml ← scene hasil rakitan (bisa dibuka di MuJoCo)
   ├─ joint_traj.json     ← trajektori sendi siap moveJ()
   └─ plan_report.txt     ← laporan validasi

3. Instalasi (Windows + VSCode)

1. Install Python 3.10+ dan ekstensi Python di VSCode.
2. Buka folder simulasi/ di VSCode (File ▸ Open Folder).
3. Buat virtual env (Terminal ▸ New Terminal):

   python -m venv .venv
   .\.venv\Scripts\Activate.ps1
   pip install -r requirements.txt

   ur_rtde di requirements.txt hanya perlu untuk execute_real.py. Untuk sim/planning saja, cukup mujoco, numpy, pyyaml.

4. Model UR5e sudah disertakan di mujoco_menagerie/. Jika ingin meng-update:

   git clone https://github.com/google-deepmind/mujoco_menagerie
   

   lalu arahkan robot.menagerie_ur5e_dir di config ke folder universal_robots_ur5e.

4. Menjalankan simulasi

python plan.py                # plan + smoothing + validasi + export ke generated/
python plan.py --benchmark    # + laporan success-rate (N run, lihat planner.benchmark_runs)
python plan.py --viewer       # + buka viewer animasi setelah plan
python run_viewer.py          # animasi trajektori (plan ulang lalu putar)
python run_viewer.py --start  # tampilkan konfigurasi START statis
python run_viewer.py --goal   # tampilkan konfigurasi GOAL statis

Cek config valid tanpa planning:

python -m ur5planner.config_loader config.yaml

Hasil sukses (contoh):

Benchmark 20 run: success_rate=100%, iter rata2=105
[OK] 5 waypoint semuanya bebas tabrakan.
[OK] semua segmen waypoint bebas tabrakan (res=0.05 rad).
[OK] N sampel trajektori bebas tabrakan.
[OK] kecepatan/akselerasi puncak <= limit.
Hasil akhir: NOL TABRAKAN / VALID

5. Bagaimana collision-check bekerja (penting & jujur)

Scene ur5e bawaan menagerie meng-exclude banyak pasangan self-collision demi performa simulasi. Kalau planner mengandalkan jumlah contact MuJoCo (data.ncon), self-collision bisa lolos diam-diam → "nol tabrakan" jadi janji palsu.

Karena itu planner ini tidak memakai contact bawaan. Ia memakai mj_geomDistance (jarak geometris antar geom, independen dari contype/exclude) dengan daftar pasangan yang dibangun eksplisit + margin per pasangan:

• robot vs tiap obstacle → margin = obstacle_inflation (digelembungkan)
• robot vs platform & lantai → margin = platform_margin (link mount dikecualikan)
• self-collision link tak bersebelahan → margin = self_collision_margin

Tabrakan = jarak < margin (mj_geomDistance < 0 berarti saling menembus). Butuh mujoco >= 3.1.4.

6. Konvensi frame

• Frame dunia sim = frame base robot. Origin = titik mount base UR, z ke atas.
• Top platform diasumsikan di z = 0; base robot di base.position (default origin).
• Obstacle/goal Anda ukur relatif terhadap base, sumbu mengikuti konvensi base UR (sama dengan yang dipakai model menagerie). Karena replay dikirim sebagai sudut sendi, hanya bagian IK goal & posisi obstacle yang sensitif frame — sudut sendi sendiri bebas-frame.

7. Mengganti placeholder dengan ukuran nyata

Edit config.yaml, cari semua tanda [GANTI]:

• platform.center/size — dimensi & posisi meja alumunium (m).
• base.position, base.yaw_deg — posisi & rotasi mount base.
• obstacles[] — tiap obstacle box: center & size (m, frame base). Wajib minimal satu obstacle yang benar-benar menghalangi jalur start→goal.
• goal.position + goal.quat_wxyz — pose TCP target (mode cartesian → IK). Atau set goal.mode: joints dan isi goal.joints untuk lewati IK (lebih robust).
• real_robot.ip — IP controller UR.
• obstacle_inflation — margin keamanan (default 0.04 m); naikkan jika kalibrasi kasar atau ada gripper/flange tebal.

Setelah edit, jalankan python plan.py --benchmark dan pastikan NOL TABRAKAN / VALID.

8. Eksekusi ke UR5 fisik (execute_real.py)

pip install ur_rtde
python execute_real.py --dry-run            # WAJIB pertama: speed/accel sangat rendah
python execute_real.py --dry-run --replan   # re-plan dari getActualQ() lebih dulu
python execute_real.py --normal             # kecepatan normal (HANYA setelah dry-run aman)

Alur skrip: konek RTDEControl+RTDEReceive → getActualQ() sebagai start → (opsional --replan dari start nyata) → konfirmasi manual → moveJ(path) dengan speed/accel dari config.real_robot. Skrip meminta ketik ya di tiap langkah.

--replan memakai kode planning yang sama dengan sim, dari posisi robot saat ini → trajektori benar-benar berangkat dari kondisi nyata (disarankan jika start nyata berbeda dari asumsi rencana).

9. ✅ Checklist serah-terima SIM → REAL

1. Semua [GANTI] di config.yaml sudah diisi dari pengukuran fisik nyata.
2. python plan.py --benchmark → success_rate tinggi & NOL TABRAKAN / VALID.
3. Lihat python run_viewer.py — jalur masuk akal & jelas menghindar obstacle.
4. obstacle_inflation cukup besar untuk menyerap error kalibrasi + gripper.
5. real_robot.ip benar; PC terhubung ke jaringan controller; bisa ping.
6. Area kerja kosong dari manusia/benda tak termodelkan. E-STOP terjangkau.
7. Robot di posisi start yang aman; konfirmasi getActualQ() masuk akal.
8. python execute_real.py --dry-run — amati seluruh gerak pelan.
9. Hanya jika dry-run mulus → naikkan kecepatan (config.real_robot.normal) → python execute_real.py --normal.
10. Jika start nyata ≠ rencana, gunakan --replan.

10. UR5e vs UR5 CB3

Model sim selalu UR5e (menagerie). Untuk replay joint-space, varian hampir tidak berpengaruh karena yang dikirim adalah sudut sendi. CB3 punya panjang link sedikit berbeda → selisih Cartesian kecil, diserap oleh obstacle_inflation. Set robot.variant dan, bila perlu, sesuaikan max_joint_velocity/accel. ur_rtde mendukung CB3 maupun e-Series.

11. Non-goals / batasan

Tanpa sensing/feedback runtime (open-loop). Tanpa torque/impedance/RL. Tanpa ROS/MoveIt/Gazebo. Tanpa gripper. IK numerik bisa gagal untuk pose ekstrem (pakai goal.mode: joints sebagai fallback). Profil waktu trapesium berhenti di tiap waypoint; pemulusan sudut nyata diserahkan ke blend radius moveJ.

---

Tugas 2: Track → Ambil → Kembalikan (closed-loop, SIM)

Manipulator & lingkungan sama (UR5e + platform + obstacle), ditambah gripper Robotiq 2F-85, sebuah objek bebas, dan kamera di gripper. Lengan menatap objek (kamera mengikuti arahnya seperti kepala ular); begitu objek ditinggalkan (diam beberapa detik), lengan mengambil dan mengembalikannya ke posisi awal.

Beda mendasar dengan Tugas 1

Ini closed-loop: tiap langkah membaca posisi objek (ground-truth dari sim). Karena itu ia hanya simulasi — tidak bisa di-replay open-loop ke robot nyata seperti plan.py. Untuk ke robot nyata butuh sensor/persepsi (kamera+deteksi), di luar lingkup ini.

Cara menjalankan

pip install -r requirements.txt          # mujoco, numpy, pyyaml (scipy opsional)
python track_and_retrieve.py             # INTERAKTIF: seret objek di viewer (Ctrl+drag kanan)
python track_and_retrieve.py --auto      # objek bergerak otomatis lalu ditinggalkan
python track_and_retrieve.py --headless --seconds 30   # tanpa display (verifikasi cepat)

Di mode interaktif: Ctrl + drag tombol kanan mouse untuk menyeret objek biru. Saat Anda menyeret, lengan menatapnya; saat Anda lepas & objek diam ~1.5 s, lengan mengambilnya dan menaruhnya kembali di kotak hijau (home).

Mesin status (FSM)

TRACK → APPROACH → DESCEND → GRASP → LIFT → CARRY → PLACE → RELEASE → RETREAT → TRACK

• TRACK (gaze): tahan posisi "perch", arahkan sumbu +z tool (kamera) ke objek. Pantau kecepatan objek; bila < tracking.idle_speed selama tracking.idle_time dan objek belum di home → mulai ambil.
• GRASP: tutup gripper + aktifkan weld objek↔gripper.
• RELEASE: buka gripper + matikan weld di posisi home. Tiap fase punya timeout pengaman 8 s agar tak deadlock.

File terkait (Tugas 2)

track_and_retrieve.py          ← main loop (interaktif / --auto / --headless)
ur5planner/object_scene.py     ← scene: UR5e + 2F-85 + objek bebas + kamera + weld
ur5planner/track_control.py    ← baca state, IK aim/reach, grasp (weld)
ur5planner/track_states.py     ← FSM track→ambil→kembalikan
ur5planner/auto_object.py      ← driver objek otomatis (mode --auto)
mujoco_menagerie/robotiq_2f85/ ← model gripper (disertakan)

Parameter di config.yaml (bagian bawah): gripper, object, tracking, pick, home_return, work_surface.

Catatan jujur tentang desain (penting)

1. Grasp = weld kinematik, bukan murni kontak gesekan. Gripper 2F-85 menutup secara fisik (terlihat), TAPI pegangan dijamin oleh equality-weld MuJoCo yang di-ON saat grasp. Grasp kontak murni di MuJoCo rapuh (slip/gaya tutup); weld membuat demo andal & objek tak terlepas. Untuk grasp kontak realistis, matikan weld di track_control.set_grasp dan tuning friction/force gripper.

2. Ditambahkan "meja kerja" (work_surface). Platform utama berada di bawah base & terlalu sempit di arah +y, sehingga objek di area kerja akan jatuh. Saya tambahkan permukaan statis di depan robot (dalam jangkauan) tempat objek bertumpu. Ganti angkanya sesuai meja nyata Anda jika perlu.

3. "Kamera mengikuti seperti ular". Diimplementasikan sebagai gaze control: lengan menahan posisi perch dan hanya mereorientasi agar sumbu kamera menunjuk objek — kepala/gripper bergerak mengikuti objek, badan lengan relatif diam. Kamera gripper_cam terpasang searah sumbu pendekatan tool.

4. Verifikasi. --headless mencetak transisi state & error posisi akhir objek vs home. Pada uji 30 s (mode auto), tiga siklus penuh terjadi dan objek kembali ke home dengan error ~0.002 m. Lihat generated/track_verify.png.