Matematika Kinematika Robotika — Blue Dragon Security

01

Pendahuluan & Konsep Dasar

Kinematika robot adalah ilmu yang mempelajari gerak robot tanpa mempertimbangkan gaya atau torsi yang menyebabkan gerak tersebut. Kinematika berfokus pada hubungan geometrik antara konfigurasi joint dan posisi/orientasi end-effector di ruang Cartesian.

⚙️

Forward Kinematics (FK)

Diberikan sudut/posisi joint → tentukan posisi dan orientasi end-effector di dunia nyata.

🎯

Inverse Kinematics (IK)

Diberikan posisi/orientasi target → tentukan sudut joint yang harus dicapai robot.

📐

Velocity Kinematics

Hubungan antara kecepatan joint (joint velocity) dengan kecepatan end-effector via Jacobian.

🔗

Kinematic Chain

Serial linkage: serangkaian rigid link yang dihubungkan oleh revolute atau prismatic joint.

1.1 Derajat Kebebasan (DOF)

DOF (Degrees of Freedom) adalah jumlah variabel independen yang diperlukan untuk mendeskripsikan konfigurasi sistem.

Grübler-Kutzbach Formula DOF = λ(n - 1 - j) + Σfᵢ Dimana: λ = 6 (ruang 3D), 3 (ruang 2D) n = jumlah link (termasuk ground) j = jumlah joint fᵢ = DOF joint ke-i Revolute joint \to fᵢ = 1 Prismatic joint \to fᵢ = 1 Ball-socket \to fᵢ = 3 Contoh robot 6-DOF serial: DOF = 6(7 - 1 - 6) + 6\times1 = 0 + 6 = 6

ℹ️

Catatan penting: Robot dengan 6 DOF memiliki kemampuan untuk menempatkan end-effector di posisi (x,y,z) dan orientasi (roll,pitch,yaw) mana pun di dalam workspace-nya. Robot redundan memiliki DOF > 6.

1.2 Sistem Koordinat

Konvensi yang digunakan adalah sistem koordinat kanan (right-handed) sesuai standar ISO 9283:

FIG-01 ▸ Sistem Koordinat Kanan (Right-Handed Coordinate System)

02

Transformasi Koordinat & Rotasi

2.1 Matriks Rotasi 3D

Rotasi murni di ruang 3D direpresentasikan oleh matriks ortogonal 3×3 dengan determinan = +1. Tiga rotasi dasar:

Rotasi terhadap sumbu X (Roll)

Rx(α) — Rotasi sumbu X ⎡ 1 0 0 ⎤ Rₓ(α) = ⎢ 0 cos(α) -sin(α) ⎥ ⎣ 0 sin(α) cos(α) ⎦

Rotasi terhadap sumbu Y (Pitch)

Ry(β) — Rotasi sumbu Y ⎡ cos(β) 0 sin(β) ⎤ Ry(β) = ⎢ 0 1 0 ⎥ ⎣ -sin(β) 0 cos(β) ⎦

Rotasi terhadap sumbu Z (Yaw)

Rz(γ) — Rotasi sumbu Z ⎡ cos(γ) -sin(γ) 0 ⎤ Rz(γ) = ⎢ sin(γ) cos(γ) 0 ⎥ ⎣ 0 0 1 ⎦

✅

Properti penting: R⁻¹ = Rᵀ (inverse = transpose). Rotasi komposit: R = R₁·R₂·R₃ — urutan perkalian penting (tidak komutatif)!

2.2 Homogeneous Transformation Matrix

Matriks 4×4 yang menggabungkan rotasi (R) dan translasi (p) dalam satu representasi:

Homogeneous Transformation Matrix T ⎡ R₃ₓ₃ | p₃ₓ₁ ⎤ ⎡ r₁₁ r₁₂ r₁₃ pₓ ⎤ T = ⎢ ——————|—————⎥ = ⎢ r₂₁ r₂₂ r₂₃ py ⎥ ⎣ 0ᵀ | 1 ⎦ ⎢ r₃₁ r₃₂ r₃₃ pz ⎥ ⎣ 0 0 0 1 ⎦ R = matriks rotasi 3\times3 p = vektor translasi [px, py, pz]ᵀ Baris terakhir [0 0 0 1] = konstanta Invers matriks transformasi: T⁻¹ = ⎡ Rᵀ | -Rᵀp ⎤ ⎣ 0ᵀ | 1 ⎦

Komposisi Transformasi

T₀ₙ = T₀₁ \cdot T₁₂ \cdot T₂₃ \cdot ... \cdot Tₙ₋₁,ₙ Posisi end-effector: p = T₀ₙ[0:3, 3] Orientasi end-effector: R = T₀ₙ[0:3, 0:3]

2.3 Representasi Orientasi

Euler Angles (ZYZ Convention)

ZYZ Euler Angles R(φ,θ,ψ) = Rz(φ) \cdot Ry(θ) \cdot Rz(ψ) Ekstrak dari matriks R: θ = atan2(\sqrt(r₁₃²+r₂₃²), r₃₃) φ = atan2(r₂₃/sin θ, r₁₃/sin θ) ψ = atan2(r₃₂/sin θ, -r₃₁/sin θ)

Roll-Pitch-Yaw (RPY / ZYX)

RPY — Extrinsic ZYX Convention R(r,p,y) = Rz(y) \cdot Ry(p) \cdot Rx(r) Ekstrak RPY dari matriks R: roll (r) = atan2(r₃₂, r₃₃) pitch (p) = atan2(-r₃₁, \sqrt(r₃₂²+r₃₃²)) yaw (y) = atan2(r₂₁, r₁₁) ⚠ Gimbal Lock terjadi saat pitch = \pm90°

Quaternion (Representasi Stabil)

Unit Quaternion q = [w, x, y, z] q = w + xi + yj + zk, w² + x² + y² + z² = 1 Axis-angle \to Quaternion: w = cos(θ/2) x = kx\cdotsin(θ/2) y = ky\cdotsin(θ/2) z = kz\cdotsin(θ/2) Quaternion \to Matriks Rotasi: R = ⎡ 1-2(y²+z²) 2(xy-wz) 2(xz+wy) ⎤ ⎢ 2(xy+wz) 1-2(x²+z²) 2(yz-wx) ⎥ ⎣ 2(xz-wy) 2(yz+wx) 1-2(x²+y²) ⎦ Keunggulan: bebas gimbal lock, interpolasi slerp lebih smooth

03

Forward Kinematics (FK)

Forward kinematics menghitung posisi dan orientasi end-effector dari konfigurasi joint yang diketahui menggunakan convention Denavit-Hartenberg (D-H).

3.1 Parameter Denavit-Hartenberg (D-H)

Setiap link ke-i memiliki 4 parameter D-H yang mendeskripsikan relasi geometrik dengan link ke-(i-1):

Parameter	Simbol	Definisi	Jenis
Link Length	aᵢ	Jarak sepanjang xᵢ dari zᵢ₋₁ ke zᵢ	Konstan
Link Twist	αᵢ	Sudut dari zᵢ₋₁ ke zᵢ di sekitar xᵢ	Konstan
Link Offset	dᵢ	Jarak sepanjang zᵢ₋₁ dari xᵢ₋₁ ke xᵢ	Var (prismatic)
Joint Angle	θᵢ	Sudut dari xᵢ₋₁ ke xᵢ di sekitar zᵢ₋₁	Var (revolute)

3.2 D-H Transformation Matrix

Matriks Transformasi D-H untuk Joint ke-i Tᵢ₋₁,ᵢ = Rot(z, θᵢ) \cdot Trans(z, dᵢ) \cdot Trans(x, aᵢ) \cdot Rot(x, αᵢ) ⎡ cos θᵢ -sin θᵢ\cdotcos αᵢ sin θᵢ\cdotsin αᵢ aᵢ\cdotcos θᵢ ⎤ = ⎢ sin θᵢ cos θᵢ\cdotcos αᵢ -cos θᵢ\cdotsin αᵢ aᵢ\cdotsin θᵢ ⎥ ⎢ 0 sin αᵢ cos αᵢ dᵢ ⎥ ⎣ 0 0 0 1 ⎦ Kalikan secara berurutan untuk mendapat pose end-effector: T₀ₙ = T₀₁(θ₁) \cdot T₁₂(θ₂) \cdot T₂₃(θ₃) \cdot ... \cdot Tₙ₋₁,ₙ(θₙ)

3.3 Contoh Lengkap: Robot 2-DOF Planar

FIG-02 ▸ Robot Planar 2-DOF

D-H Parameter Tabel — Robot 2-DOF Joint | θᵢ | dᵢ | aᵢ | αᵢ ──────|─────|────|────|──── 1 | θ₁ | 0 | L₁ | 0 2 | θ₂ | 0 | L₂ | 0 Matriks T₀₁: T₀₁ = ⎡ cos θ₁ -sin θ₁ 0 L₁cos θ₁ ⎤ ⎢ sin θ₁ cos θ₁ 0 L₁sin θ₁ ⎥ ⎢ 0 0 1 0 ⎥ ⎣ 0 0 0 1 ⎦ Matriks T₁₂: T₁₂ = ⎡ cos θ₂ -sin θ₂ 0 L₂cos θ₂ ⎤ ⎢ sin θ₂ cos θ₂ 0 L₂sin θ₂ ⎥ ⎢ 0 0 1 0 ⎥ ⎣ 0 0 0 1 ⎦ Pose End-Effector T₀₂ = T₀₁ \cdot T₁₂: px = L₁cos θ₁ + L₂cos(θ₁+θ₂) py = L₁sin θ₁ + L₂sin(θ₁+θ₂) Orientasi total = θ₁ + θ₂

3.4 Robot 6-DOF (PUMA-Style) — D-H Tabel

Joint	θᵢ	dᵢ	aᵢ	αᵢ	Keterangan
1	θ₁	d₁	0	90°	Waist / Base rotation
2	θ₂	0	a₂	0°	Shoulder
3	θ₃	0	a₃	90°	Elbow
4	θ₄	d₄	0	-90°	Wrist roll
5	θ₅	0	0	90°	Wrist pitch
6	θ₆	d₆	0	0°	Wrist yaw / tool

04

Jacobian & Velocity Kinematics

Jacobian adalah matriks yang menghubungkan kecepatan joint dengan kecepatan end-effector. Sangat penting untuk IK numerik dan kontrol robot.

4.1 Jacobian Geometrik

Velocity Kinematics ẋ = J(q) \cdot q̇ ⎡ ṗx ⎤ ⎡ J_L ⎤ ⎢ ṗy ⎥ ⎢ J_L ⎥ (6\timesn matrix) ⎢ ṗz ⎥ = ⎢ ... ⎥ \cdot q̇ₙₓ₁ ⎢ ωx ⎥ ⎢ J_A ⎥ ⎢ ωy ⎥ ⎢ J_A ⎥ ⎣ ωz ⎦ ⎣ J_A ⎦ J_L = kolom linear velocity (translasi) J_A = kolom angular velocity (rotasi) n = jumlah joint Kolom ke-i Jacobian: Untuk revolute joint: Jᵢ_L = zᵢ₋₁ \times (pₙ - pᵢ₋₁) Jᵢ_A = zᵢ₋₁ Untuk prismatic joint: Jᵢ_L = zᵢ₋₁ Jᵢ_A = 0 Dimana zᵢ₋₁ = kolom ke-3 R₀,ᵢ₋₁ (sumbu z frame i-1) pᵢ₋₁ = posisi origin frame i-1 pₙ = posisi end-effector

Contoh Jacobian 2-DOF Planar

J = ⎡ -L₁sin θ₁ - L₂sin(θ₁+θ₂) -L₂sin(θ₁+θ₂) ⎤ ⎣ L₁cos θ₁ + L₂cos(θ₁+θ₂) L₂cos(θ₁+θ₂) ⎦ det(J) = L₁\cdotL₂\cdotsin θ₂ Singularitas terjadi saat θ₂ = 0° atau 180° (arm fully extended/folded)

4.2 Singularitas

⚠️

Singularitas terjadi ketika rank(J) < 6 — Jacobian kehilangan rank. Robot kehilangan kemampuan bergerak dalam satu atau lebih arah. det(J) = 0 adalah kondisi singularitas. Harus dihindari dalam kontrol robot real-time.

Deteksi Singularitas Cek: det(J) \approx 0 \to singularitas Tiga jenis pada robot 6-DOF: 1. Wrist singularity : θ₅ = 0 (sumbu z₄ ∥ z₆) 2. Shoulder singularity: wrist center di atas/bawah joint 1 3. Elbow singularity : arm sepenuhnya ter-extend (θ₃ = 0/π) Kondisi Number (manipulability): μ = \sqrt(det(J\cdotJᵀ)) \to besar = jauh dari singularitas

05

Inverse Kinematics ★ DETAIL LENGKAP

🎯 Apa itu Inverse Kinematics?

Inverse Kinematics (IK) adalah operasi kebalikan dari FK: diberikan pose end-effector yang diinginkan (posisi + orientasi), cari nilai joint angle q = [θ₁, θ₂, ..., θₙ] yang mencapai pose tersebut. IK jauh lebih sulit dari FK karena: (1) solusi bisa tidak ada, (2) solusi bisa tidak unik (multiple solutions), (3) persamaan bersifat nonlinear dan transcendental, dan (4) tidak selalu ada bentuk closed-form.

5.1 Konsep, Tantangan, dan Klasifikasi

📊

Tidak Ada Solusi

Target berada di luar workspace robot, atau target memerlukan orientasi yang tidak bisa dicapai.

🔀

Multiple Solutions

Robot 6-DOF umumnya memiliki hingga 16 solusi berbeda untuk satu target pose.

∞

Infinite Solutions

Robot redundan (DOF > 6) memiliki infinitely many solutions — space null-space.

⚡

Real-time Constraint

Kontrol robot memerlukan IK dalam hitungan milidetik (<1ms untuk servo loop 1kHz).

Problem Formulation Diberikan: T_desired = ⎡ R_d | p_d ⎤ (target pose) ⎣ 0 | 1 ⎦ Cari: q* = [θ₁*, θ₂*, ..., θₙ*]ᵀ Sehingga: FK(q*) = T_desired Secara matematis: q* = FK⁻¹(T_desired) Error function: e(q) = T_desired ⊖ FK(q) ⎡ Δpx ⎤ ⎡ p_d - p(q) ⎤ \leftarrow position error (3D) ⎢ Δpy ⎥ = ⎢ ... ⎥ ⎢ Δpz ⎥ ⎣ ... ⎦ ⎢ Δωx ⎥ ⎢ Δωy ⎥ \leftarrow orientation error (axis-angle dari R_d\cdotR(q)ᵀ) ⎣ Δωz ⎦

5.2 Metode Analitik (Closed-Form)

Metode analitik menghasilkan persamaan eksplisit untuk setiap joint angle. Lebih cepat dari numerik tapi hanya tersedia untuk struktur robot tertentu.

✅

Pieper's Theorem: Closed-form IK selalu ada untuk robot 6-DOF jika: (1) 3 joint terakhir berpotongan di satu titik (spherical wrist), ATAU (2) 3 joint berurutan adalah prismatic. Sebagian besar robot industri mengikuti kondisi ini.

5.3 IK Analitik 2-DOF Planar — Langkah Demi Langkah

1

Setup Problem — Definisikan Target

Diberikan posisi target end-effector (px, py) dan panjang link L₁, L₂. Tujuan: cari θ₁ dan θ₂.

Target: (px, py) Parameter: L₁, L₂ Dari FK: px = L₁cos θ₁ + L₂cos(θ₁+θ₂) ... (1) py = L₁sin θ₁ + L₂sin(θ₁+θ₂) ... (2)

2

Cari θ₂ — Hukum Cosinus

Kuadratkan dan jumlahkan persamaan (1) dan (2):

px² + py² = (L₁cos θ₁ + L₂cos(θ₁+θ₂))² + (L₁sin θ₁ + L₂sin(θ₁+θ₂))² Setelah ekspansi dan penyederhanaan identitas trigonometri: px² + py² = L₁² + L₂² + 2\cdotL₁\cdotL₂\cdotcos θ₂ Isolasi cos θ₂: cos θ₂ = (px² + py² - L₁² - L₂²) / (2\cdotL₁\cdotL₂) Definisikan: D = (px² + py² - L₁² - L₂²) / (2\cdotL₁\cdotL₂) Kondisi solusi: |D| \leq 1 \to target dapat dicapai |D| > 1 \to target di luar workspace (NO SOLUTION) Hitung θ₂: θ₂ = atan2(\pm\sqrt(1 - D²), D) + \to "elbow up" (konfigurasi siku di atas) - \to "elbow down" (konfigurasi siku di bawah)

3

Cari θ₁ — Decompose Angle

Dengan θ₂ sudah diketahui, gunakan persamaan (1) dan (2):

Definisikan: k₁ = L₁ + L₂\cdotcos θ₂ k₂ = L₂\cdotsin θ₂ Persamaan (1) dan (2) menjadi: px = k₁\cdotcos θ₁ - k₂\cdotsin θ₁ ... (1') py = k₁\cdotsin θ₁ + k₂\cdotcos θ₁ ... (2') Ini adalah sistem linear dalam cos θ₁ dan sin θ₁. Selesaikan dengan rotasi sudut: θ₁ = atan2(py, px) - atan2(k₂, k₁) atan2 diperlukan untuk mendapatkan sudut di quadrant yang tepat Jangan gunakan atan biasa — kehilangan informasi tanda!

4

Dua Solusi Valid

Setiap target memiliki dua solusi (jika ada solusi):

Solusi 1 — "Elbow Up": θ₂⁺ = atan2(+\sqrt(1-D²), D) k₁⁺ = L₁ + L₂\cdotcos θ₂⁺ k₂⁺ = L₂\cdotsin θ₂⁺ θ₁⁺ = atan2(py, px) - atan2(k₂⁺, k₁⁺) Solusi 2 — "Elbow Down": θ₂⁻ = atan2(-\sqrt(1-D²), D) k₁⁻ = L₁ + L₂\cdotcos θ₂⁻ k₂⁻ = L₂\cdotsin θ₂⁻ θ₁⁻ = atan2(py, px) - atan2(k₂⁻, k₁⁻) Pilih solusi berdasarkan: - Joint limit constraints - Jarak terdekat dari konfigurasi saat ini (minimum joint motion) - Obstacle avoidance

5

Verifikasi dengan Forward Kinematics

Verifikasi: FK(θ₁*, θ₂*) \approx (px, py) ? px_check = L₁\cdotcos θ₁* + L₂\cdotcos(θ₁*+θ₂*) py_check = L₁\cdotsin θ₁* + L₂\cdotsin(θ₁*+θ₂*) error = \sqrt((px - px_check)² + (py - py_check)²) Terima solusi jika error < threshold (misal: 1e-6 meter)

5.4 IK Analitik 3-DOF Planar

Untuk robot 3-DOF planar, kita juga perlu mengontrol orientasi φ = θ₁ + θ₂ + θ₃:

1

Hitung Posisi "Wrist" (Joint 3)

Target: (px, py, φ_desired) Link lengths: L₁, L₂, L₃ Hitung posisi sebelum link L₃ (wrist point): pwx = px - L₃\cdotcos(φ) pwy = py - L₃\cdotsin(φ) Masalah kini tereduksi ke 2-DOF IK ke (pwx, pwy)!

2

Selesaikan 2-DOF IK untuk θ₁ dan θ₂

Gunakan prosedur 2-DOF IK pada (pwx, pwy): D = (pwx² + pwy² - L₁² - L₂²) / (2\cdotL₁\cdotL₂) θ₂ = atan2(\pm\sqrt(1-D²), D) k₁ = L₁ + L₂\cdotcos θ₂ k₂ = L₂\cdotsin θ₂ θ₁ = atan2(pwy, pwx) - atan2(k₂, k₁)

3

Hitung θ₃ dari Constraint Orientasi

θ₃ = φ_desired - θ₁ - θ₂ Verifikasi orientasi: φ_check = θ₁ + θ₂ + θ₃ = φ_desired ✓

5.5 Wrist Decoupling untuk Robot 6-DOF

Metode paling efisien untuk robot 6-DOF dengan spherical wrist (joint 4, 5, 6 berpotongan di satu titik):

1

Hitung Posisi Wrist Center (WC)

Target pose: T_desired = [R_d | p_d] Posisi wrist center adalah offset d₆ ke belakang dari end-effector: p_wc = p_d - d₆ \cdot R_d \cdot [0, 0, 1]ᵀ p_wc = p_d - d₆ \cdot n̂_z n̂_z = kolom ke-3 dari R_d (arah z tool frame) d₆ = offset dari wrist center ke ujung tool Komponen: wc_x = p_dx - d₆\cdotr₁₃ wc_y = p_dy - d₆\cdotr₂₃ wc_z = p_dz - d₆\cdotr₃₃

2

Selesaikan θ₁, θ₂, θ₃ dari Posisi WC

Solve for θ₁ (waist rotation): θ₁ = atan2(wc_y, wc_x) atau θ₁ = atan2(wc_y, wc_x) + π (konfigurasi "flipped") Solve for θ₃ (elbow): r = \sqrt(wc_x² + wc_y²) - a₂ (jarak horizontal dari joint 2) s = wc_z - d₁ (jarak vertikal dari joint 2) D₃ = (r² + s² - a₂² - a₃²) / (2\cdota₂\cdota₃) θ₃ = atan2(\pm\sqrt(1-D₃²), D₃) Solve for θ₂ (shoulder): θ₂ = atan2(s, r) - atan2(a₃\cdotsin θ₃, a₂ + a₃\cdotcos θ₃)

3

Hitung Matriks Rotasi Wrist R₃₆

Dari θ₁, θ₂, θ₃ yang sudah diketahui, hitung R₀₃: R₀₃ = R₀₁(θ₁) \cdot R₁₂(θ₂) \cdot R₂₃(θ₃) Isolasi rotasi wrist: R₃₆ = (R₀₃)⁻¹ \cdot R_desired = (R₀₃)ᵀ \cdot R_desired R₃₆ adalah matriks 3\times3 yang hanya bergantung pada θ₄, θ₅, θ₆ Bentuk R₃₆ untuk robot dengan ZYZ wrist: R₃₆ = ⎡ c₄c₅c₆-s₄s₆ -c₄c₅s₆-s₄c₆ c₄s₅ ⎤ ⎢ s₄c₅c₆+c₄s₆ -s₄c₅s₆+c₄c₆ s₄s₅ ⎥ ⎣ -s₅c₆ s₅s₆ c₅ ⎦

4

Ekstrak θ₄, θ₅, θ₆ dari R₃₆

Bandingkan elemen R₃₆ dengan bentuk analitik: Jika sin θ₅ \neq 0 (kasus umum, bukan singularitas): θ₅ = atan2(\sqrt(R₃₆[0,2]² + R₃₆[1,2]²), R₃₆[2,2]) = atan2(\sqrt(r₁₃² + r₂₃²), r₃₃) Solusi 1 (θ₅ > 0): θ₄ = atan2(R₃₆[1,2]/sin θ₅, R₃₆[0,2]/sin θ₅) = atan2(r₂₃/sin θ₅, r₁₃/sin θ₅) θ₆ = atan2(R₃₆[2,1]/sin θ₅, -R₃₆[2,0]/sin θ₅) = atan2(r₃₂/sin θ₅, -r₃₁/sin θ₅) Solusi 2 (θ₅ < 0): negasi θ₅, lalu koreksi θ₄ dan θ₆: θ₅' = -θ₅ θ₄' = atan2(-r₂₃/sin θ₅, -r₁₃/sin θ₅) θ₆' = atan2(-r₃₂/sin θ₅, r₃₁/sin θ₅) Jika sin θ₅ = 0 (SINGULARITAS WRIST): θ₅ = 0, hanya (θ₄ + θ₆) yang bisa ditentukan \to Set θ₄ = θ₄_current, θ₆ = target_sum - θ₄ \to Atau gunakan DLS untuk melewati singularitas

5

Pilih Konfigurasi Terbaik

Robot 6-DOF spherical wrist menghasilkan hingga 8 solusi: Konfigurasi | θ₁ | θ₃ | θ₅ ─────────────|───────|─────────|────── Config 1 | atan2 | elbow+ | + Config 2 | atan2 | elbow+ | - Config 3 | atan2 | elbow- | + Config 4 | atan2 | elbow- | - Config 5 | flip | elbow+ | + Config 6 | flip | elbow+ | - Config 7 | flip | elbow- | + Config 8 | flip | elbow- | - Strategi pemilihan: 1. Filter konfigurasi yang melanggar joint limits 2. Hitung Δq = |q_desired - q_current| untuk setiap solusi valid 3. Pilih solusi dengan Δq minimum (minimum joint motion) 4. Optionally: pilih yang jauh dari singularitas (max manipulability)

5.6 Metode Numerik — Overview

Metode numerik mengiterasi dari konfigurasi awal menuju solusi. Lebih general dari analitik — berlaku untuk robot dengan struktur apa pun termasuk redundan.

Metode	Kecepatan	Akurasi	Singularitas	Use Case
Newton-Raphson	Cepat	Tinggi	Gagal	Far from singularity
Jacobian Pseudoinverse	Sedang	Tinggi	Degraded	Redundant robots
Damped Least Squares	Sedang	Baik	Robust	General purpose
CCD	Cepat	Sedang	OK	Real-time, games
FABRIK	Sangat Cepat	Baik	OK	Character animation

5.7 Newton-Raphson IK

Metode iteratif menggunakan linearisasi Jacobian untuk memperbaiki estimasi joint angle:

1

Definisikan Error Function

e(q) = x_desired - FK(q) Dimana x_desired = [px, py, pz, ωx, ωy, ωz]ᵀ (6D task space) Dan FK(q) = posisi + orientasi saat ini Untuk orientasi, gunakan axis-angle error: e_orient = R_desired \otimes R_current⁻¹ \to konversi ke axis-angle vector

2

Update Rule

Δq = J⁻¹ · e(q) (untuk n=6 robot square) Iterasi: q_{k+1} = q_k + α · Δq_k α = step size (biasanya 0.1 - 1.0) Lebih kecil α → lebih stabil, lebih lambat konvergen

3

Kondisi Berhenti

Stop jika: ‖e(q)‖ < ε_pos (misal: 0.001 meter) AND ‖e_orient‖ < ε_rot (misal: 0.01 radian) atau max_iterations tercapai (misal: 100 iter) atau ‖Δq‖ < ε_joint (perubahan sangat kecil)

⚙ ALGORITMA — Newton-Raphson IK

function NewtonRaphsonIK(T_desired, q_init, max_iter, eps): q ← q_init for k = 1 to max_iter: T_curr ← ForwardKinematics(q) e_pos ← T_desired[0:3,3] - T_curr[0:3,3] e_rot ← AxisAngleError(T_desired[0:3,0:3], T_curr[0:3,0:3]) e ← [e_pos; e_rot] if norm(e) < eps: return q, SUCCESS J ← ComputeJacobian(q) Δq ← inv(J) · e ← WARNING: J singular = fail q ← q + α·Δq q ← ClampToJointLimits(q) return q, FAILED_MAX_ITER

5.8 Jacobian Pseudoinverse

Untuk robot non-square (DOF ≠ 6) atau robot redundan, gunakan Moore-Penrose pseudoinverse:

Pseudoinverse (J⁺) Kasus over-determined (m > n): lebih banyak equations dari unknowns J⁺ = Jᵀ(JJᵀ)⁻¹ \to minimum norm solution Kasus under-determined (m < n): robot redundan, DOF > task dim J⁺ = (JᵀJ)⁻¹Jᵀ \to least squares solution Update rule: Δq = J⁺ \cdot e + (I - J⁺J) \cdot z (I - J⁺J) = null-space projector z = arbitrary vector \to digunakan untuk secondary objectives: - joint limit avoidance - singularity avoidance - manipulability maximization

5.9 Damped Least Squares (DLS) — Levenberg-Marquardt

Metode paling robust untuk IK numerik. Menambahkan damping factor λ untuk regularisasi di dekat singularitas:

1

Persamaan DLS

DLS Solution: Δq = Jᵀ \cdot (J\cdotJᵀ + λ²\cdotI)⁻¹ \cdot e Equivalent to minimizing: min ‖J\cdotΔq - e‖² + λ²\cdot‖Δq‖² λ kecil \to seperti pseudoinverse (akurat, tidak stabil di singularitas) λ besar \to pergerakan kecil, sangat stabil, konvergen lambat Variable damping (adaptive λ): λ² = λ_max² \cdot (1 - (μ/μ_max)²) μ = manipulability = \sqrtdet(JJᵀ) Jauh dari singularitas: μ besar \to λ kecil (akurat) Dekat singularitas: μ kecil \to λ besar (stabil)

2

Tuning Parameter λ

Pilih: λ_max \in [0.01, 0.1] (tergantung skala robot) μ_max \approx nilai manipulability arm fully extended Rule of thumb untuk robot industri: λ² = 0 jika μ > 0.1\cdotμ_max λ² = λ_max² \cdot exp(-μ/0.05) otherwise

⚙ ALGORITMA — Damped Least Squares IK

function DLS_IK(T_desired, q_init, λ_max, max_iter, eps): q ← q_init for k = 1 to max_iter: T_curr ← ForwardKinematics(q) e ← TaskSpaceError(T_desired, T_curr) if norm(e) < eps: return q, SUCCESS J ← ComputeJacobian(q) μ ← sqrt(det(J @ Jᵀ)) ← manipulability λ² ← ComputeDamping(μ, λ_max) Δq ← Jᵀ @ inv(J @ Jᵀ + λ²·I) @ e q ← ClampJointLimits(q + α·Δq) return q, NOT_CONVERGED

5.10 Cyclic Coordinate Descent (CCD)

Metode iteratif yang mengoptimasi satu joint pada satu waktu, berulang dari end-effector ke base:

1

Ide Dasar CCD

Untuk setiap joint i (dari ujung ke base), rotasikan joint i sehingga end-effector sedekat mungkin ke target:

Vektor dari joint i ke EE: v₁ = EE_pos - joint_i_pos Vektor dari joint i ke target: v₂ = target_pos - joint_i_pos Sudut rotasi optimal: Δθᵢ = atan2(cross(v₁, v₂), dot(v₁, v₂)) Untuk 3D dengan sumbu rotasi: axis = normalize(v₁ \times v₂) angle = acos(dot(v₁, v₂) / (‖v₁‖\cdot‖v₂‖)) Δθᵢ diprojeksi ke sumbu joint

2

Loop CCD

CCD LOOP

for iter = 1 to max_iter: for i = n-1 downto 0: ← dari joint terakhir ke base v1 ← EE_pos(q) - joint_pos(i, q) v2 ← target_pos - joint_pos(i, q) Δθ ← signed_angle(v1, v2, joint_axis(i)) Δθ ← clamp(Δθ, -Δθ_max, Δθ_max) ← joint velocity limit q[i] ← clamp(q[i] + Δθ, q_min[i], q_max[i]) if dist(EE_pos(q), target) < eps: break

⚠️

Limitasi CCD: Tidak menjamin konvergensi global. Bisa terjebak di local minimum. Untuk orientasi end-effector, perlu modifikasi khusus. CCD hanya optimal untuk position-only IK.

5.11 FABRIK (Forward and Backward Reaching IK)

Algoritma FABRIK sangat cepat, intuitif, dan menghasilkan hasil yang natural. Ideal untuk real-time animation dan soft robotics:

1

Forward Reaching Pass

Gerakkan end-effector ke target, tarik seluruh chain: p[n] \leftarrow target_pos \leftarrow pindahkan EE ke target for i = n-1 downto 0: d \leftarrow ‖p[i+1] - p[i]‖ λ \leftarrow link_length[i] / d p[i] \leftarrow (1-λ)\cdotp[i+1] + λ\cdotp[i] \leftarrow interpolasi ke arah lama

2

Backward Reaching Pass

Kembalikan base ke posisi aslinya, propagate ke depan: p[0] \leftarrow base_pos \leftarrow kembalikan base ke posisi fixed for i = 0 to n-1: d \leftarrow ‖p[i+1] - p[i]‖ λ \leftarrow link_length[i] / d p[i+1] \leftarrow (1-λ)\cdotp[i] + λ\cdotp[i+1] \leftarrow ikatan link length

3

Iterasi Hingga Konvergen

while dist(p[n], target) > eps AND iter < max_iter: ForwardPass(p) BackwardPass(p) iter++ Konversi posisi ke joint angles: θᵢ = atan2(p[i+1].y - p[i].y, p[i+1].x - p[i].x) - θ_parent FABRIK konvergen dalam 5-10 iterasi untuk sebagian besar kasus!

5.12 Perbandingan Lengkap Semua Metode IK

Aspek	Analitik	Newton-Raphson	DLS	CCD	FABRIK
Kecepatan	★★★★★	★★★★	★★★	★★★★	★★★★★
Akurasi	Eksak	Sangat tinggi	Tinggi	Sedang	Tinggi
Generalitas	Terbatas	Medium	Tinggi	Tinggi	Tinggi
Near singularity	✓ (OK)	✗ (gagal)	✓ (robust)	✓ (OK)	✓ (OK)
Joint limits	Manual	Post-clamp	Post-clamp	Built-in	Partial
Redundant robots	✗	Partial	✓ (null-space)	✓	✓
Implementasi	Kompleks	Sedang	Sedang	Mudah	Sangat mudah
Rekomendasi	Robot industri 6-DOF	Non-singular paths	General purpose	Game/Realtime	Animation/Soft

06

Workspace Analysis

Workspace adalah himpunan semua pose yang dapat dicapai end-effector. Penting untuk memverifikasi feasibility sebelum menjalankan IK.

6.1 Jenis Workspace

🌐

Reachable Workspace

Himpunan semua posisi (x,y,z) yang bisa dicapai end-effector dalam orientasi apa pun.

🎯

Dexterous Workspace

Subset dari reachable workspace dimana end-effector dapat dicapai dalam orientasi apa pun.

6.2 Cek Feasibility Sebelum IK

Pengecekan Workspace 2-DOF r = \sqrt(px² + py²) \leftarrow jarak target dari base Kondisi reachable: |L₁ - L₂| \leq r \leq L₁ + L₂ r < |L₁ - L₂| \to target terlalu dekat (inside inner workspace) r > L₁ + L₂ \to target terlalu jauh (outside outer workspace) Pengecekan umum: D = (px² + py² - L₁² - L₂²) / (2\cdotL₁\cdotL₂) Feasible ⟺ -1 \leq D \leq 1

07

Implementasi Python

7.1 Kelas Robot Kinematics Lengkap

import numpy as np
from numpy.linalg import norm, inv, det, pinv

class RobotKinematics:
    """Robot Kinematics — Blue Dragon Security Research
    Mendukung: FK, IK (Analitik 2DOF, DLS Numerik)
    """

    def __init__(self, dh_params, joint_limits):
        # dh_params: list of (theta_offset, d, a, alpha)
        self.dh = dh_params
        self.n  = len(dh_params)
        self.q_min = np.array([lim[0] for lim in joint_limits])
        self.q_max = np.array([lim[1] for lim in joint_limits])

    def dh_matrix(self, theta, d, a, alpha):
        """Single D-H transformation matrix."""
        ct, st = np.cos(theta), np.sin(theta)
        ca, sa = np.cos(alpha), np.sin(alpha)
        return np.array([
            [ct, -st*ca,  st*sa, a*ct],
            [st,  ct*ca, -ct*sa, a*st],
            [ 0,     sa,     ca,    d],
            [ 0,      0,      0,    1]
        ])

    def forward_kinematics(self, q):
        """FK: joint angles → end-effector pose T (4x4)."""
        T = np.eye(4)
        for i, (th_off, d, a, alpha) in enumerate(self.dh):
            T = T @ self.dh_matrix(q[i] + th_off, d, a, alpha)
        return T

    def jacobian(self, q, delta=1e-6):
        """Numerical Jacobian via finite differences."""
        T0 = self.forward_kinematics(q)
        p0 = T0[:3, 3]
        J  = np.zeros((6, self.n))
        for i in range(self.n):
            q2 = q.copy(); q2[i] += delta
            T2 = self.forward_kinematics(q2)
            # Linear velocity
            J[:3, i] = (T2[:3,3] - p0) / delta
            # Angular velocity (from skew-symmetric)
            dR = (T2[:3,:3] - T0[:3,:3]) / delta @ T0[:3,:3].T
            J[3, i] = dR[2,1]
            J[4, i] = dR[0,2]
            J[5, i] = dR[1,0]
        return J

    def ik_dls(self, T_des, q_init, lam=0.05,
                  max_iter=200, eps_pos=1e-4, eps_rot=1e-3):
        """Damped Least Squares Inverse Kinematics."""
        q = q_init.copy()
        for _ in range(max_iter):
            T = self.forward_kinematics(q)
            # Position error
            e_pos = T_des[:3,3] - T[:3,3]
            # Orientation error (axis-angle)
            R_err = T_des[:3,:3] @ T[:3,:3].T
            angle = np.arccos(np.clip((np.trace(R_err)-1)/2, -1, 1))
            if abs(angle) < 1e-9:
                e_rot = np.zeros(3)
            else:
                e_rot = angle/(2*np.sin(angle)) * np.array(
                    [R_err[2,1]-R_err[1,2],
                     R_err[0,2]-R_err[2,0],
                     R_err[1,0]-R_err[0,1]])
            e = np.concatenate([e_pos, e_rot])
            if norm(e_pos) < eps_pos and norm(e_rot) < eps_rot:
                return q, True
            J     = self.jacobian(q)
            lam2  = lam**2
            dq    = J.T @ np.linalg.solve(J @ J.T + lam2*np.eye(6), e)
            q     = np.clip(q + dq, self.q_min, self.q_max)
        return q, False  # max iter reached

########## Contoh Penggunaan ##########
if __name__ == "__main__":
    # Robot 2-DOF sederhana
    L1, L2 = 0.5, 0.4
    dh = [(0, 0, L1, 0),  # joint 1
          (0, 0, L2, 0)]  # joint 2
    limits = [(-np.pi, np.pi)] * 2
    robot = RobotKinematics(dh, limits)

    # FK test
    q_test = [np.pi/4, np.pi/3]
    T = robot.forward_kinematics(q_test)
    print(f"FK pos: {T[:3,3]}")

    # IK test
    T_target = T.copy()
    q_init = np.zeros(2)
    q_sol, ok = robot.ik_dls(T_target, q_init)
    print(f"IK solution: {np.degrees(q_sol)} deg, converged={ok}")

7.2 IK Analitik 2-DOF (Fast Path)

def ik_analytic_2dof(px, py, L1, L2):
    """Closed-form IK untuk robot 2-DOF planar.
    Returns: list of (theta1, theta2) solutions, atau [] jika unreachable
    """
    D = (px**2 + py**2 - L1**2 - L2**2) / (2*L1*L2)
    if abs(D) > 1.0:
        return []  # Tidak ada solusi

    solutions = []
    for sign in [+1, -1]:
        theta2 = np.arctan2(sign * np.sqrt(1 - D**2), D)
        k1     = L1 + L2 * np.cos(theta2)
        k2     = L2 * np.sin(theta2)
        theta1 = np.arctan2(py, px) - np.arctan2(k2, k1)
        solutions.append((theta1, theta2))

    return solutions  # [(elbow_up), (elbow_down)]

# Test
sols = ik_analytic_2dof(0.6, 0.3, 0.5, 0.4)
for i, (t1, t2) in enumerate(sols):
    print(f"Sol {i+1}: θ₁={np.degrees(t1):.2f}°, θ₂={np.degrees(t2):.2f}°")

REF

Referensi & Bacaan Lanjutan

Sumber	Judul	Topik
Spong et al.	Robot Modeling and Control (2nd Ed.)	FK, IK, Jacobian, Dynamics
Craig, J.J.	Introduction to Robotics (3rd Ed.)	D-H Parameters, FK, IK
Lynch & Park	Modern Robotics	Screw theory, IK numerik
Siciliano et al.	Robotics: Modelling, Planning and Control	Komprehensif
Kenwright, B.	FABRIK (Robotics & Automation Letters)	FABRIK algorithm
Wampler, C.W.	Manipulator IK via DLS	DLS / Levenberg-Marquardt
Buss, S.R.	Introduction to IK with Jacobian Transpose	Jacobian methods
robotics.stackexchange.com	Community Q&A	Praktis/implementasi

Matematika Kinematikauntuk Robotika

▸ Daftar Isi

Pendahuluan & Konsep Dasar

1.1 Derajat Kebebasan (DOF)

1.2 Sistem Koordinat

Transformasi Koordinat & Rotasi

2.1 Matriks Rotasi 3D

Rotasi terhadap sumbu X (Roll)

Rotasi terhadap sumbu Y (Pitch)

Rotasi terhadap sumbu Z (Yaw)

2.2 Homogeneous Transformation Matrix

Komposisi Transformasi

2.3 Representasi Orientasi

Euler Angles (ZYZ Convention)

Roll-Pitch-Yaw (RPY / ZYX)

Quaternion (Representasi Stabil)

Forward Kinematics (FK)

3.1 Parameter Denavit-Hartenberg (D-H)

3.2 D-H Transformation Matrix

3.3 Contoh Lengkap: Robot 2-DOF Planar

3.4 Robot 6-DOF (PUMA-Style) — D-H Tabel

Jacobian & Velocity Kinematics

4.1 Jacobian Geometrik

Contoh Jacobian 2-DOF Planar

4.2 Singularitas

Inverse Kinematics ★ DETAIL LENGKAP

🎯 Apa itu Inverse Kinematics?

5.1 Konsep, Tantangan, dan Klasifikasi

5.2 Metode Analitik (Closed-Form)

5.3 IK Analitik 2-DOF Planar — Langkah Demi Langkah

5.4 IK Analitik 3-DOF Planar

5.5 Wrist Decoupling untuk Robot 6-DOF

5.6 Metode Numerik — Overview

5.7 Newton-Raphson IK

5.8 Jacobian Pseudoinverse

5.9 Damped Least Squares (DLS) — Levenberg-Marquardt

5.10 Cyclic Coordinate Descent (CCD)

5.11 FABRIK (Forward and Backward Reaching IK)

5.12 Perbandingan Lengkap Semua Metode IK

Workspace Analysis

6.1 Jenis Workspace

6.2 Cek Feasibility Sebelum IK

Implementasi Python

7.1 Kelas Robot Kinematics Lengkap

7.2 IK Analitik 2-DOF (Fast Path)

Referensi & Bacaan Lanjutan

Matematika Kinematika
untuk Robotika