07 · Rigid transform'lar ve frame tree'leri


Bölüm	2 · Spatial math
Çalışma türü	CPU · Mac ya da Colab CPU
Önkoşul	06
Önce öğren	Vectors, Matrices & Operations · Matrix Transformations
Süre	~45 dk
Kod	`experiments/07-rigid-transforms/experiment.py`
Durum	✅ tam

Bu derste yap

Her adımı yapınca işaretle. Yalnız bu tarayıcıda saklanır.

01 · Oku Problem ve ana fikir; okumadan önceki soruları cevapla.
02 · Sıfırdan yaz Adımları kendin yaz, sonra çalıştır uv run python experiments/07-rigid-transforms/build.py
03 · Deneyi çalıştır Önce tahmin et, sonra çalıştır uv run python experiments/07-rigid-transforms/experiment.py
04 · Boz Tek parametreyi değiştir ve bozulmasını izle.
05 · Kendini dene Sondaki quiz ve alıştırmalar.

Bir nokta, ancak frame’inin adıyla birlikte bir sayı üçlüsüdür; transform o adı değiştirme yoludur.

Problem

Menagerie SO-101 modelinde bir bilek kamerası var (wrist_cam, dikey görüş açısı 61°, so101.xml içinde gripper’a tanımlı). Bir detector bu görüntüde bir küp buluyor. Küpü kavramak için kolun küpü base frame’inde bilmesi gerekir; çeneleri kapatmak için gripper frame’inde; başka robotlarla birlikte kaydetmek için world frame’inde. Bunların her biri aynı fiziksel nokta için farklı bir sayı üçlüsüdür ve her joint hareketinde değişir.

Bunları karıştırmak hiçbir şeyi çökertmez. Gripper’ı sessizce küpten 10 ila 42 cm uzağa gönderir (aşağıda ölçüldü). Bu ders dört transform’u elle kuruyor ve MuJoCo’ya karşı kontrol ediyor.

Okumadan önce · 2 soru. Önce tahmin et; nedenini ders anlatıyor.

1. Küp hareket etmiyor. shoulder_pan’i ±30° döndürüyorsun. Küpün koordinatları hangi frame’lerde değişir?

Her frame’de, çünkü robot hareket etti.
Yalnızca base frame’inde, çünkü pan joint’i base’in üstünde.
Yalnızca gripper ve kamera frame’lerinde, ikisinde de aynı mesafe kadar.

Neden: Bir joint koordinatları yalnızca ağaçta kendisinden sonraki frame’lerde değiştirir. C bölümü, W ve B’de 0,00 mm, G ve C’de 93,77 mm; $T_{GC}$ sabit.

2. Gripper’ın taban frame’indeki pose’unu, $T_{BG}$ , ve kameranın gripper frame’indeki pose’unu, $T_{GC}$ , biliyorsun. Kamerayı taban frame’inde nasıl bulursun?

$T_{GC}\,T_{BG}$ , çünkü önce kamera uygulanır.
$T_{BC} = T_{BG}\,T_{GC}$ ; içteki harfler birimler gibi sadeleşir.
$T_{BG} + T_{GC}$ , iki ofseti toplayarak.

Neden: Bileşke, “A from B” sırasında çarpımdır (Ana fikir tablosu). Sıfırdan yaz, Adım 5 $T_{WB}\,T_{BG} = T_{WG}$ zincirini 1,1e-16 farkla doğruluyor; Boz’daki yanlış sıralı satır 103,2 mm sapıyor.

Ana fikir

Sade anlatımla

“Küp 15 cm önümde” demek, ancak “ben”in nerede olduğunu ve hangi yöne baktığımı biliyorsan işe yarar. Bunu herkes için aynı anlama gelen bir GPS koordinatıyla karşılaştır. Frame böyle bir referanstır: bir orijin ve üç eksen yönü. World frame sahnenin GPS’idir; base, gripper ve kamera frame’leri ise robotun her parçası için “önümde”dir. Aynı küpün her birinde farklı bir sayı üçlüsü vardır.

Rigid transform iki frame arasında çeviri tarifidir: döndür (ders 06), sonra kaydır. Gripper’ın base frame’inde nerede olduğunu ve kameranın gripper frame’inde nerede olduğunu biliyorsan, iki tarifi zincirleyip kamerayı base frame’inde bulursun. Kodda bu düz function composition’dır: world_from_base(base_from_gripper(gripper_from_camera(p))). Transform’ları “A from B” diye adlandırmak zinciri dürüst tutar: komşu harfler birimler gibi eşleşmelidir.

Bir robot böyle frame’lerden oluşan bir ağaçtır, frame tree. Bazı kenarlar sabittir (base’in cıvatalandığı yer, kameranın gripper’a vidalandığı yer); diğerleri bir joint döndükçe değişir.

flowchart TD
    W["world W"] -- "T_WB: base yerleşimi<br/>(sabit)" --> B["base B"]
    W -- "sabit" --> K["küp"]
    B -- "shoulder → upper arm → lower arm<br/>→ wrist → gripper<br/>(5 joint, q ile değişir)" --> G["gripper site'ı G"]
    G -- "T_GC: kamera bağlantısı<br/>(sabit)" --> C["bilek kamerası C"]
    C -- "pinhole projeksiyonu" --> I["pikseller u, v"]

Transform’lar kod olarak

import numpy as np

# T(R, t) [R t; 0 1] kurar; T_inv(M) = [R^T  -R^T t; 0 1] (Sıfırdan yaz, Adım 1 ikisini de yazıyor)
# Adlandırma: T_AB, B frame'indeki koordinatları A frame'ine taşır ("A from B").
p_B = np.array([x, y, z, 1.0])   # homogeneous nokta: 1 ekle ki kaydırma da çarpımın parçası olsun
p_A = T_AB @ p_B                 # B -> A
T_AC = T_AB @ T_BC               # zincir = bileşke; içteki harfler sadeleşir
p_B = T_inv(T_AB) @ p_A          # A -> B

$B$ frame’inin $A$ frame’indeki pose’u, bir $R_{AB}$ rotation’ı ve $B$ ‘nin orijininin $A$ ‘daki konumu $t_{AB}$ ‘dir; ikisi 4×4’lük bir homogeneous transform’da toplanır:

$T_{AB} = \begin{bmatrix} R_{AB} & t_{AB} \\ 0 & 1\end{bmatrix},\qquad p_A = T_{AB}\,p_B\ \ (p = [x\ y\ z\ 1]^\top \text{ ile}).$

İşlem	Formül	Pratik kural
Bileşke	$T_{AC} = T_{AB}\,T_{BC}$	içteki alt indisler sadeleşir
Ters	$T_{AB}^{-1} = T_{BA} = \begin{bmatrix} R^\top & -R^\top t \\ 0 & 1\end{bmatrix}$	genel matris tersi çağırma
Noktanın frame’ini değiştir	$p_B = T_{AB}^{-1}\,p_A$	istediğin alt indis sağa gelir

$T_{AB}$ , $B$ ‘deki koordinatları $A$ ‘daki koordinatlara taşıyan transform’dur; $R_{AB}$ onun 3×3’lük rotation’ı, $t_{AB}$ metre cinsinden translation’ıdır; $p_A$ ile $p_B$ aynı fiziksel noktanın, sonuna 1 eklenmiş olarak $A$ ve $B$ frame’lerindeki yazılışıdır.

Matematik ayrıntısı: ters neden ucuz, mesafeler neden korunur

Homogeneous koordinatlar. Bir noktaya 1 eklemek, “döndür, sonra $t$ ekle” işlemini tek bir matris çarpımına çevirir: $T p = [Rp + t;\ 1]$ . Transform zincirlerini bir çarpım zinciri yapan budur.

Kapalı form ters. $p_A = R p_B + t$ denklemini $p_B$ için çöz: $p_B = R^{-1}(p_A - t) = R^\top p_A - R^\top t$ , çünkü rotation matrix $R^\top R = I$ ‘yı sağlar. Bu kesindir ve genel tersten ucuzdur; deney bunu np.linalg.inv ile karşılaştırıyor.

Mesafeler korunur. İki nokta için $\lVert T p - T p'\rVert = \lVert R(p - p')\rVert = \lVert p - p'\rVert$ . Yani iki frame birbirine rigid olarak bağlıysa (aralarındaki transform sabitse), herhangi bir nokta ikisinde de aynı mesafe kadar oynar: Sonuç 3 bunu gripper ve kamera için gösteriyor.

Bir joint hareket edince hangi koordinatlar değişir. $p_F = T_{WF}^{-1}\,p_W$ . Bir joint $T_{WF}$ ‘yi ancak ağaçta world’den $F$ ‘ye giden yol üzerindeyse değiştirir; $F$ ‘den sonra gelen bir joint $F$ koordinatlarını değiştiremez.

Daha derine: AIEFS Vectors, Matrices & Operations, Matrix Transformations; Lynch & Park, Modern Robotics, Böl. 3.

MuJoCo’da frame tree’ler

MuJoCo her düğümün world pose’unu hesaplayıp mjData’da saklar (API reference):

Nesne	Konum	Orientation	Dolduran
body (normal frame)	`xpos`	`xmat`, `xquat`	`mj_kinematics`
body inertial frame’i	`xipos`	`ximat`	`mj_kinematics`
site	`site_xpos`	`site_xmat`	`mj_kinematics`
kamera	`cam_xpos`	`cam_xmat`	`mj_camlight` (`mj_forward`’un parçası)

Site, bir body’ye bağlı, adı olan ve kütlesi olmayan bir frame’dir; gripper ucu gibi noktaları işaretlemek için kullanılır. Ezberlenecek iki konvansiyon. MuJoCo kamerası kendi frame’inin −z ekseni boyunca bakar; görüntüde +x sağ, +y yukarıdır (XML reference: camera). Bir framepos sensor’ünde objtype/reftype="body" body’nin inertial frame’i (orijini kütle merkezinde), "xbody" ise normal frame’idir (XML reference: framepos).

Sıfırdan yaz

Şimdi 4 × 4 transform’ları ve MuJoCo’nun frame ağacında aşağı yürümeyi yaklaşık 50 satır NumPy ile yazacak, Deney sahnesinin her frame’ini yalnızca model dosyasından ve qpos’tan hesaplayacak ve her adımı MuJoCo ile karşılaştıracaksın. Model Deney’inki: tabanı taşınmış ve 30° döndürülmüş SO-101, masada serbest bir küp. Adımları kendi dosyana yaz ve her adımdan sonra çalıştır. Bitmiş fonksiyonlar physai/spatial.py içinde, ders 06’nın rotation’larının yanında; uv run python experiments/07-rigid-transforms/build.py beş adımı kontrolleriyle birlikte koşturur (çıktı build_output.txt dosyasında).

Adım 1 · Bir transform, tersi, bileşkesi

def transform(R, t):
    """4x4 homogeneous transform from a 3x3 rotation R and a translation t."""
    T = np.eye(4)
    T[:3, :3], T[:3, 3] = np.asarray(R).reshape(3, 3), t
    return T

def transform_inv(T):
    """Closed-form inverse [R t; 0 1]^-1 = [R^T  -R^T t; 0 1]."""
    R, t = T[:3, :3], T[:3, 3]
    return transform(R.T, -R.T @ t)

def transform_point(T, p):
    """Apply T to a point: R p + t (the homogeneous 1 is implicit)."""
    return T[:3, :3] @ p + T[:3, 3]

MuJoCo pose’ları bir konum ve skaler önde bir quaternion olarak saklar; ders 06’nın quat_to_mat’i birini transform’a çevirir:

def pose_to_transform(pos, quat):
    """MuJoCo's (pos, quat) pair, quat scalar first, as a 4x4 transform."""
    return transform(quat_to_mat(quat), pos)

MuJoCo’nun kendi pose cebiri mju_negPose (ters) ve mju_mulPose (bileşke):

== Step 1. Transforms: inverse and composition (1000 random poses) ==
transform_inv vs np.linalg.inv   1.6e-15
transform_inv vs mju_negPose     4.4e-16
T_a @ T_b vs mju_mulPose         1.1e-15

Adım 2 · Frame ağacında yürü

Her body’nin pose’u, parent’ının pose’u çarpı sabit ofseti (body_pos, body_quat, qpos0’daki pose) çarpı joint başına bir hareket. Hinge, joint’in anchor’ı jnt_pos’tan geçen ekseni etrafında döner, yani dönüş “anchor’a git” ile “geri gel” arasına sıkıştırılır; slide öteler; free joint’in qpos’u doğrudan dünyadaki pose’tur.

def frame_tree(m, qpos):
    """World pose (4x4) of every body, walking MuJoCo's tree from static model data and qpos alone."""
    T = [np.eye(4)]                                    # body 0 is the world
    for b in range(1, m.nbody):
        Tb = T[m.body_parentid[b]] @ pose_to_transform(m.body_pos[b], m.body_quat[b])   # pose at qpos0
        for j in range(m.body_jntadr[b], m.body_jntadr[b] + m.body_jntnum[b]):
            a, kind = m.jnt_qposadr[j], m.jnt_type[j]
            if kind == 0:                              # free: qpos holds the world pose itself
                Tb = pose_to_transform(qpos[a:a + 3], qpos[a + 3:a + 7])
                continue
            anchor = transform(np.eye(3), m.jnt_pos[j])
            if kind == 1:                              # ball: rotate by the quaternion about the anchor
                move = pose_to_transform(np.zeros(3), qpos[a:a + 4])
            elif kind == 2:                            # slide: translate along the axis
                move = transform(np.eye(3), m.jnt_axis[j] * (qpos[a] - m.qpos0[a]))
            else:                                      # hinge: rotate about the axis through the anchor
                move = transform(axis_angle_to_mat(m.jnt_axis[j], qpos[a] - m.qpos0[a]), np.zeros(3))
            Tb = Tb @ anchor @ move @ transform_inv(anchor)
        T.append(Tb)
    return T

Body’ler parent’ları çocuklarından önce gelecek şekilde saklanır, yani index sırasıyla tek geçiş yeter:

== Step 2. Frame-tree walk vs MuJoCo's xpos / xmat (every body, 50 random arm poses) ==
10 bodies (world, 8 SO-101 links, the free cube) · max |pos error| 2.2e-16 m · max |rotation error| 1.0e-15

Yürüyüşün, serbest küp dahil on body’nin hepsi için mj_kinematics’i sabit model sayılarından ve qpos’tan yeniden üretiyor.

Adım 3 · Body’lere asılı frame’ler

Site’lar, kameralar ve bir body’nin inertial frame’i bir body’ye göre sabit ofsetlerdir: her biri için bir çarpım daha.

T_WG = T[m.site_bodyid[s]] @ pose_to_transform(m.site_pos[s], m.site_quat[s])     # gripper site
T_WC = T[m.cam_bodyid[c]] @ pose_to_transform(m.cam_pos[c], m.cam_quat[c])        # wrist camera
T_WI = T[b] @ pose_to_transform(m.body_ipos[b], m.body_iquat[b])                 # inertial frame

== Step 3. Frames hanging off bodies: site, camera, inertial frame ==
site gripperframe    max error 4.4e-16
camera wrist_cam     max error 6.4e-16
base inertial frame  max error 3.5e-15

Adım 4 · Küp her frame’de

“Küp F frame’inde” demek $T_{WF}^{-1}\,p_W$ demek:

== Step 4. The cube in each frame: our transforms vs the framepos sensors ==
base B                0.2200  0.0000  0.0150 m · max |ours - sensor| 2.8e-17
gripper G             0.0806 -0.0290 -0.0205 m · max |ours - sensor| 1.0e-16
camera C             -0.0064 -0.0011 -0.1508 m · max |ours - sensor| 1.1e-16
base INERTIAL frame  -0.0044  0.0397  0.2032 m · max |ours - sensor| 7.4e-16
inertial frame vs regular frame of the base: 295.5 mm apart

Deney’in Sonuç 1’i, bu kez mjData’dan tek bir pose okumadan; 295,5 mm’lik tuzak da açıklandı: reftype="body", ofseti ve rotation’ı body_ipos, body_iquat olan tabanın inertial frame’inden ölçer.

Adım 5 · Kullan

Kamera frame’indeki noktayla Sonuç 2’nin pinhole formülü pikselleri verir; gripper transform’uyla Boz’daki her hata tek bir yanlış satırdır:

== Step 5. Use the transforms: pixels, and five ways to get the cube into the gripper frame ==
f = 407.4 px · cube centre at u = 302.7, v = 242.8 px (camera looks along -z, image v grows downward)
T_WG^-1 p (correct)              error    0.0 mm
R (p - t): R instead of R^T      error  101.0 mm
(T_BG T_WB)^-1 p: wrong order    error  103.2 mm
R^T p: forgot the translation    error  274.3 mm
T_WG p: transform, not inverse   error  424.4 mm
chain: max |T_WB @ T_BG - T_WG| = 1.1e-16

Deney’in sayıları, kendi transform’larından.

Elinde kalan

physai.spatial artık transform, transform_inv, transform_point, pose_to_transform ve frame_tree içeriyor. Ders 08 bu yürüyüşü kolun forward kinematics’ine çeviriyor, ders 19 kamera transform’u $T_{GC}$ ‘yi kalibre ediyor. tests/test_07_rigid_transforms.py bunları MuJoCo’ya sabitliyor; bir şey değiştirdikten sonra uv run pytest tests/test_07_rigid_transforms.py çalıştır.

Kütüphaneyle kullan

MuJoCo bütün ağacı mj_kinematics’te (kameraları mj_camlight’ta; mj_forward ikisini de çağırır) hesaplar ve sonuçları mjData’da bırakır:

mujoco.mj_forward(m, d)
T_WB = transform(d.xmat[b], d.xpos[b])                  # body b (regular frame)
T_WG = transform(d.site_xmat[s], d.site_xpos[s])        # site s
T_WC = transform(d.cam_xmat[c], d.cam_xpos[c])          # camera c: looks along its -z
pos, quat = np.zeros(3), np.zeros(4)
mujoco.mju_mulPose(pos, quat, pos_a, quat_a, pos_b, quat_b)   # compose without 4x4 matrices
mujoco.mju_negPose(pos, quat, pos_a, quat_a)                  # invert

Araç	Pose’u nasıl saklar	Kamera nereye bakar
physai	4 × 4 `T_AB` (B’den A’ya)	–
MuJoCo	`xpos` + `xmat` (9 sayı, row-major) ya da `xquat` (skaler önde)	−z, +y yukarı
OpenCV	`rvec` (axis-angle) + `tvec`, dünyadan kameraya	+z, +y aşağı (Gerçek robotta)

Tuzaklar:

Bayat frame’ler. xpos, xmat ancak son mj_kinematics/mj_forward kadar tazedir; qpos’u ayarlayıp çağrıyı unutursan her frame bir önceki pozunkidir.
mj_kinematics kameraları atlar. cam_xpos, cam_xmat mj_forward’ın çalıştırdığı mj_camlight’tan gelir (Deney script’i bu yüzden mj_forward kullanıyor).
Body ve inertial frame. xpos/xmat body frame’i, xipos/ximat kütle merkezi frame’idir (Adım 4: burada 295,5 mm ayrı).

Yazdıktan sonra · 2 soru.

1. frame_tree’de bir hinge’in dönüşü yalnızca move değil anchor @ move @ transform_inv(anchor) olarak yazılıyor. Neden?

Hinge, body orijininden değil joint’in anchor’ı jnt_pos’tan geçen ekseni etrafında döner; sandviç oraya gider, döner ve geri gelir.
Her joint’ten sonra rotation matrisini normalize etmek için.
Çünkü MuJoCo rotation’ları transpoze saklar.

Neden: Ders 09’un $o + T(p - o)$ ‘su ile aynı fikir. Bununla yürüyüş on body’nin hepsi için xpos ve xmat’i 2,2e-16 m ve 1,0e-15 farkla yeniden üretiyor (Sıfırdan yaz, Adım 2).

2. Serbest küp için frame_tree body’nin pose’unu parent’ın pose’una çarpmak yerine qpos ile değiştiriyor. Bu neden doğru?

Çünkü küpün kütlesi yok.
Bir free joint’in yedi qpos sayısı, body’nin dünyadaki konumu ve skaler önde quaternion’ının ta kendisidir.
Çünkü free joint’ler diğer bütün body’lerden sonra hesaplanır.

Neden: Bir free body’nin nq = 7 ama nv = 6 olmasının sebebi de bu (ders 06). Yürüyüş küp için de her kol linki gibi MuJoCo ile örtüşüyor; küpün konumu Adım 4’ün sensörlerle karşılaştırmasına giriyor.

Deney

Script: experiments/07-rigid-transforms/experiment.py. MuJoCo 3.17.0, NumPy 2.5.4, Apple Silicon Mac (Apple M5), deterministik, yaklaşık 2 s.

Kurulum. SO-101 base’i, MuJoCo’nun modeli kodla düzenleme API’si MjSpec ile (0,10, −0,05, 0) m’ye taşındı ve 30° yaw verildi; yani world ≠ base. 3 cm’lik bir küp masada, world’de (0,2905, 0,0600, 0,015) m’de duruyor. Kol, bilek kamerasının küpü gördüğü bir pozda. Dört framepos sensor’ü küpü base (xbody), base (body), gripper site’ı ve kamera frame’inde raporluyor.

Önce tahmin et. shoulder_pan’i döndürünce (hareketsiz) küpün dört koordinat üçlüsünden hangileri değişir?

def T_inv(M):
    R, t = M[:3, :3], M[:3, 3]
    return T(R.T, -R.T @ t)

F = {"B": T(d.xmat[base], d.xpos[base]),
     "G": T(d.site_xmat[site], d.site_xpos[site]),
     "C": T(d.cam_xmat[cam], d.cam_xpos[cam])}
cube_in = {k: (T_inv(Tw) @ np.append(p_world, 1))[:3] for k, Tw in F.items()}

Sonuç 1 · elle transform ile MuJoCo.

Frame	elle küp (m)	sensor’e göre en büyük mutlak fark
world W	(0,2905, 0,0600, 0,0150)	–
base B	(0,2200, 0,0000, 0,0150)	2,8e-17
gripper G (site `gripperframe`)	(0,0806, −0,0290, −0,0205)	1,4e-17
kamera C (`wrist_cam`)	(−0,0064, −0,0011, −0,1508)	4,1e-17

Kapalı formdaki ters, np.linalg.inv ile 1,1e-15’e kadar tutuyor; $T_{WB}T_{BG}$ de $T_{WG}$ ‘yi 5,6e-17’ye kadar veriyor. Kamera üçlüsünde $z = -0{,}1508$ m: küp lensin 15 cm önünde, neredeyse optik eksen (lensten dümdüz çıkan çizgi) üzerinde.

Sonuç 2 · kamera frame’inden piksele. Bir pinhole kamera derinliğe böler: iki kat uzaktaki bir nokta, görüntü merkezine iki kat yakın düşer. 640×480’de odak uzaklığı $f = (H/2)/\tan(\text{fovy}/2) = 407{,}4$ px ile küp merkezi $u = 302{,}7$ , $v = 242{,}8$ px’e düşüyor ( $u = W/2 + f x/(-z)$ , $v = H/2 - f y/(-z)$ ). Küpün 8 köşesini projekte edip convex hull’larını doldurmak, MuJoCo’nun render ettiği segmentation maskesine karşı IoU’su (kesişim alanı bölü birleşim alanı) 0,979 olan bir siluet veriyor (9643’e karşı 9454 px), centroid farkı 0,74 px. Doğru kontrol centroid değil siluettir (Gerçek robotta bölümüne bak).

Sonuç 3 · shoulder_pan sweep’i. Diğer joint’ler sabit; koordinatlar cm.

pan	küp B’de	küp G’de	küp C’de	gripper site’ı W’de
−40°	22,00, 0,00, 1,50	5,30, 2,62, 11,15	12,62, 2,90, −9,83	18,88, 9,21, 8,26
0°	22,00, 0,00, 1,50	8,16, −1,46, −0,19	1,23, 1,14, −14,39	25,47, 2,79, 8,26
40°	22,00, 0,00, 1,50	5,30, −10,68, −7,96	−6,62, −8,08, −17,01	26,40, −6,36, 8,26

Sonra her kol joint’i pozun etrafında ±30° sweep ediliyor (yalnızca kinematik, limitler uygulanmıyor):

Sweep edilen joint	W’de en büyük değişim	B’de	G’de	C’de	$T_{GC}$ kayması
shoulder_pan	0,00 mm	0,00 mm	93,77 mm	93,77 mm	7,8e-16
shoulder_lift	0,00	0,00	93,44	93,44	7,8e-16
elbow_flex	0,00	0,00	150,07	150,07	7,8e-16
wrist_flex	0,00	0,00	124,53	124,53	5,6e-16
wrist_roll	0,00	0,00	15,74	15,74	5,6e-16

Bir frame’deki koordinatlar, yalnızca ağaçta o frame ile world arasında bir joint varsa değişir. Base koordinatları hiç değişmiyor; gripper ve kamera koordinatları tam olarak aynı mesafe kadar değişiyor, çünkü $T_{GC}$ sabit (kayma ≤ 7,8e-16) ve rigid transform mesafeleri korur. wrist_roll gripper frame koordinatlarını en az değiştiriyor (15,74 mm): roll ekseni etrafında 30°‘ye kadar dönmek onları $2r\sin 15°$ kadar oynatır; $r$ küpün o eksene uzaklığıdır, yani küp eksenden yalnızca yaklaşık 30 mm uzakta.

Boz

Küpü gripper frame’inde hesaplamanın beş yolu, sensor’e karşı puanlanmış:

Hesap	hata
$T_{WG}^{-1}\,p$ (doğru)	0,0 mm
$R\,(p - t)$ : Rᵀ yerine R (active/passive karışıklığı)	101,0 mm
$(T_{BG}\,T_{WB})^{-1}\,p$ : yanlış bileşke sırası	103,2 mm
$R^\top p$ : translation unutulmuş	274,3 mm
$T_{WG}\,p$ : tersi yerine transform’un kendisi	424,4 mm

Bir de sinsi olanı: reftype="body" olan bir framepos sensor’ü (−0,0044, 0,0397, 0,2032) m okuyor; gerçek base frame değerinden 295,5 mm uzakta. Çünkü base’in inertial frame’ini kullanıyor: orijini kütle merkezinde, eksenleri asal atalet eksenleri. Bu hataların hiçbiri exception fırlatmaz.

Gerçek robotta

Donanımda framepos sensor’ü yoktur; $T_{WB}$ (base’in nereye cıvatalandığı) ve $T_{GC}$ (kameranın gripper üzerinde nerede durduğu) kalibrasyonla ölçülmelidir (ders 19’a bak). Modeldeki $T_{GC}$ bir CAD değeridir. Gerçek kolda henüz ölçülmedi.
OpenCV’nin pinhole modeli $u = f_x X_c/Z_c + c_x$ , $v = f_y Y_c/Z_c + c_y$ projeksiyonunu, piksel satırları aşağı doğru artacak şekilde yapar; yani +z ileri, +y aşağı (calib3d.hpp). MuJoCo kamera frame’indeki bir nokta, x etrafında 180°‘lik bir rotation ile OpenCV noktasına dönüşür: $(x, y, z) \to (x, -y, -z)$ . Bunu yanlış yapmak görüntüyü ters çevirir.
Blob centroid’ini projekte edilmiş merkez sanma. Sonuç 2’de render edilen maskenin centroid’i $u = 299{,}3$ , $v = 240{,}3$ px, merkez ise $u = 302{,}7$ , $v = 242{,}8$ px’e düşüyor: perspektifte bir katı cismin görüntüsünün centroid’i, merkezinin görüntüsü değildir. Kalibrasyon için köşe ya da keypoint kullan.

Kendini dene

Hızlı kontrol · 2 soru. Bir cevaba tıkla.

1. Küpü gripper frame’inde bulmak için biri $R^\top (p - t)$ yerine $R\,(p - t)$ yazıyor. Ne olur?

Hiçbir hata çıkmaz ve sonuç 101,0 mm sapar.
NumPy bir shape hatası fırlatır.
Hiçbir şey, çünkü rotation matrix ortogonaldir ve $R = R^\top$ .

Neden: Ortogonal, $R = R^\top$ değil $R^{-1} = R^\top$ demektir. Boz tablosunda bu active/passive karışıklığı 101,0 mm sapıyor ve hataların hiçbiri exception fırlatmıyor.

2. reftype="body" olan bir framepos sensor’ü bir noktayı gerçek base frame değerinden 295,5 mm uzakta okuyor. Neden?

Sensor konumu base frame yerine world frame’inde verir.
"body" body’nin inertial frame’i demektir, orijini kütle merkezinde ve eksenleri asal atalet eksenleri; normal frame "xbody".
Sensor’ler yalnızca iki adımda bir güncellenir.

Neden: MuJoCo’da frame tree’ler ve Boz bölümü; bu tuzak uyarı vermez, her frame sensor’ünün reftype’ını kontrol et.

Kolay. Sonuç 3’te gripper site’ının world z’si neden her pan açısında sabit (8,26 cm)?
Orta. $T_{BC}$ ‘yi T_inv ve F sözlüğü cinsinden yaz. Hangi joint’lere bağlıdır?
Zor. Küp için reftype="camera" olan bir framequat sensor’ü ekle ve $R_{CW}R_{W,\text{küp}}$ ‘ü elle doğrula. En büyük hata nedir?

Cevaplar

Pan ekseni düşey (base’e yalnızca yaw verildi); bu eksen etrafında dönmek her noktayı kendi yüksekliğinde tutar.
$T_{BC}$ = T_inv(F["B"]) @ F["C"]. Beş kol joint’inin hepsine bağlıdır, base yerleşimine hiç bağlı değildir.
Bu dersin script’inde hesaplanmadı; hatanı Sonuç 1’deki konum hatalarıyla karşılaştır.

Akılda kalanlar ve kaynaklar

Kapalı form tersle $p_B = T_{AB}^{-1}p_A$ ; bu kurulumda elle transform’lar MuJoCo ile ≤ 4,1e-17 m tuttu.
Bir joint, koordinatları yalnızca ağaçta kendisinden sonra gelen frame’lerde değiştirir; küpün base koordinatları 0,00 mm değişirken gripper/kamera koordinatları 150,07 mm’ye kadar oynadı.
Frame konvansiyonlarını bil: kamera −z ileri (MuJoCo), body = inertial frame, xbody = normal frame (295,5 mm’lik tuzak).

Sonraki: ders 08 · Forward kinematics, degrees of freedom ve workspace.

Kaynaklar

K. M. Lynch, F. C. Park, Modern Robotics, Böl. 3 “Rigid-Body Motions” (homogeneous transform’lar). Kitap sayfası.
MuJoCo: XML reference · camera, XML reference · framepos, API functions.
SO-101 modeli: MuJoCo Menagerie robotstudio_so101 (Apache-2.0).