PART 3 Python 으로 배우는 기구학 · 약 45분

10정기구학 — 관절 각도로 손끝 위치 구하기

2관절 평면 팔의 삼각함수 공식에서 출발해 4×4 변환 행렬로 reBot TCP 위치를 계산합니다.

시뮬레이터 열기 URDF 뷰어 Python Playground

정기구학이란#

모터 안의 센서가 알려 주는 것은 관절 각도뿐입니다. 그런데 우리가 궁금한 것은 "그래서 지금 손끝(그리퍼) 이 어디에 있지?" 입니다. 컵에 닿았는지, 책상에 부딪히지 않을지 알려면 각도를 손끝 위치로 바꾸는 계산이 꼭 필요합니다.

이 계산을 정기구학(Forward Kinematics, FK) 이라고 합니다. 이름은 어렵지만 뜻은 "관절 각도 → 손끝 위치" 방향으로 계산한다는 것입니다.

입력: 관절 각도 θ1 = 30° θ2 = 45° 정기구학 (FK) cos · sin · 행렬 곱 출력: 손끝 위치 (x, y) = (0.263, 0.338) 답이 딱 하나로 정해집니다 각도를 알면 손끝은 계산만으로 바로 나옵니다. 거꾸로(위치 → 각도)는 다음 장의 역기구학입니다.
정기구학의 입력과 출력. 각도가 정해지면 손끝 위치는 단 하나로 정해집니다.
  • 입력: 관절 각도 (reBot 은 6개, q1 ~ q6)
  • 출력: 손끝(TCP, Tool Center Point)의 위치 x, y, z 와 방향
  • 특징: 답이 항상 하나입니다. 같은 각도면 손끝은 언제나 같은 곳에 있습니다.

이 강좌에서 TCP 는 그리퍼 몸체의 원점입니다(RS 는 gripper_end, DM 은 end_link 링크). 시뮬레이터에서 TCP 축 보기를 켜면 그 위치에 작은 좌표축이 나타납니다. Playground 의 arm.fk() 가 바로 이 점의 좌표를 미터로 돌려줍니다.

python
arm.zero()
print("영점 자세 TCP:", [round(v, 3) for v in arm.fk()])
print("다른 자세 TCP:", [round(v, 3) for v in arm.fk([0, 90, 90, 0, 0, 0])])

RS 모델에서는 [0.302, 0.0, 0.218] 과 [0.538, 0.0, 0.454] 가 나옵니다. 이 장의 목표는 이 숫자를 우리 손으로 직접 계산해 내는 것입니다. 6개 관절을 한 번에 다루면 어려우니, 막대 하나 → 막대 둘 → reBot 전체 순서로 넓혀 갑니다.

막대 하나의 끝점 — cos 와 sin#

가장 단순한 팔은 막대 하나입니다. 한쪽 끝을 원점에 고정하고 각도 θ(세타)만큼 기울이면, 다른 끝은 직각삼각형으로 구할 수 있습니다.

xy θ L x = L·cos θ y = L·sin θ 끝점 외우는 방법 • cos 는 "옆으로(x) 얼마나 갔나" • sin 은 "위로(y) 얼마나 갔나" θ = 0° → (L, 0) 오른쪽 θ = 90° → (0, L) 위 θ =180° → (-L, 0) 왼쪽
길이 L, 각도 θ 인 막대의 끝점은 (L·cosθ, L·sinθ) 입니다.
  • cos θ: 막대 길이 중 옆(x) 으로 간 비율. 0° 에서 1, 90° 에서 0
  • sin θ: 막대 길이 중 위(y) 로 간 비율. 0° 에서 0, 90° 에서 1

9장에서 배운 대로 math.cos(), math.sin() 은 라디안을 받습니다. reBot B601-RS 의 상완 길이 0.236 m 로 해 봅시다.

python
import math

L = 0.236          # B601-RS 상완 길이 (m)
for deg in [0, 30, 45, 90, 135, 180]:
    th = math.radians(deg)
    x = L * math.cos(th)
    y = L * math.sin(th)
    print(f"θ = {deg:>3}° → 끝점 ({x:+.3f}, {y:+.3f})  거리 {math.hypot(x, y):.3f}")
text
θ =   0° → 끝점 (+0.236, +0.000)  거리 0.236
θ =  30° → 끝점 (+0.204, +0.118)  거리 0.236
θ =  45° → 끝점 (+0.167, +0.167)  거리 0.236
θ =  90° → 끝점 (+0.000, +0.236)  거리 0.236
θ = 135° → 끝점 (-0.167, +0.167)  거리 0.236
θ = 180° → 끝점 (-0.236, +0.000)  거리 0.236
검산 습관: 절대 변하지 않는 값

어떤 각도든 끝점까지의 거리(math.hypot(x, y))는 막대 길이 0.236 그대로입니다. 계산이 맞는지 자신 없을 때 이런 변하지 않는 값을 확인하는 습관이 큰 도움이 됩니다.

2관절 평면 팔 공식#

막대 두 개를 이어 봅시다. 상완(길이 L1)은 어깨에서 θ1 로 뻗고, 전완(길이 L2)은 팔꿈치에서 상완을 기준으로 θ2 만큼 꺾입니다.

핵심은 "손끝 = 팔꿈치 위치 + 전완 막대" 입니다.

  1. 팔꿈치: 상완은 막대 하나와 같으므로 (L1·cosθ1, L1·sinθ1)
  2. 전완의 방향: 상완이 이미 θ1 기울었으니 전완이 바닥과 이루는 각은 θ1 + θ2
  3. 손끝: 팔꿈치에 (L2·cos(θ1+θ2), L2·sin(θ1+θ2)) 를 더합니다
text
x = L1·cos θ1 + L2·cos(θ1 + θ2)
y = L1·sin θ1 + L2·sin(θ1 + θ2)
python
import math

L1, L2 = 0.236, 0.228     # B601-RS 상완 · 전완 (m)

def fk_2link(theta1, theta2):
    """어깨 θ1, 팔꿈치 θ2 (도) → 손끝 (x, y) (m)"""
    t1 = math.radians(theta1)
    t12 = math.radians(theta1 + theta2)
    x = L1 * math.cos(t1) + L2 * math.cos(t12)
    y = L1 * math.sin(t1) + L2 * math.sin(t12)
    return x, y

for th1, th2 in [(30, 45), (0, 0), (90, 0), (0, 90), (0, 180)]:
    x, y = fk_2link(th1, th2)
    print(f"θ1={th1:>3}°, θ2={th2:>3}° → ({x:+.3f}, {y:+.3f})")
text
θ1= 30°, θ2= 45° → (+0.263, +0.338)
θ1=  0°, θ2=  0° → (+0.464, +0.000)
θ1= 90°, θ2=  0° → (+0.000, +0.464)
θ1=  0°, θ2= 90° → (+0.236, +0.228)
θ1=  0°, θ2=180° → (+0.008, +0.000)

결과가 맞는지는 머리로 답을 아는 자세로 확인합니다.

자세θ1, θ2예상이유
쭉 편 팔0°, 0°(0.464, 0)L1 + L2 만큼 앞으로
위로 쭉90°, 0°(0, 0.464)L1 + L2 만큼 위로
ㄱ 자0°, 90°(0.236, 0.228)앞으로 L1, 위로 L2
완전히 접음0°, 180°(0.008, 0)L1 − L2 만 남음
가장 흔한 실수: θ2 만 쓰기

전완 항에 cos(θ2) 라고 쓰면 틀립니다. 관절은 언제나 부모 링크 기준으로 돌기 때문에 앞 관절의 각도가 누적됩니다. 이 "누적" 생각이 뒤에서 행렬 곱으로 이어집니다.

실습 — 팔꿈치만 돌리면 손끝은?
  1. 위 fk_2link 를 Playground 에 붙여 넣고, θ1 = 60° 로 고정한 채 θ2 를 −150° ~ 150° 까지 30° 간격으로 바꾸며 손끝을 출력하세요.
  2. 팔꿈치 (L1·cos60°, L1·sin60°) 에서 각 손끝까지의 거리를 math.hypot 로 구해 보세요. 모두 0.228 이 나오면, 손끝이 팔꿈치를 중심으로 원을 그린다는 뜻입니다.

reBot 을 옆에서 보면 — 단순화와 그 한계#

reBot 의 joint2(어깨)와 joint3(팔꿈치)는 회전축이 서로 평행합니다. 평행한 축으로 도는 관절들은 한 평면 위에서만 움직이므로, 옆에서 보면 2관절 평면 팔과 같습니다.

base_link 원점 (0, 0) J2 어깨 (0.020, 0.145) J3 팔꿈치 L1 = 0.236 L2 ≈ 0.228 J4 손목 TCP (0.302, 0.218) 옆에서 본 평면(x–z) • 0° 자세는 팔이 '접힌' 자세입니다 • 위팔은 뒤쪽(−x)으로 눕고 • 아래팔은 다시 앞으로 접힙니다 2관절 팔로 바꾸기 θ1 = 180° − q2 θ2 = q3 − 180° (q2, q3 = reBot 관절 각)
모든 관절이 0° 일 때 B601-RS 를 옆에서 본 모습(좌표는 공식 URDF 로 계산한 값, m). 상완은 뒤로 수평, 전완은 앞으로 접혀 있습니다.
2관절 팔B601-RSURDF 출처
어깨 위치 (x, z)(0.020, 0.145)joint1 z 0.075 + joint2 z 0.07
L1 (상완)0.236 mjoint3 origin x = −0.236
L2 (전완)≈ 0.228 mjoint4 origin x = 0.228 (y −0.0727 오프셋)
θ1, θ2θ1 = 180° − q2, θ2 = q3 − 180°0° 기준이 다르므로 변환

마지막 줄이 낯설지요? 우리 공식은 "θ = 0° 이면 앞으로 쭉 편 팔"이 기준이지만, reBot URDF 는 "q = 0° 이면 접힌 자세"가 기준입니다. 이렇게 로봇마다 0° 와 + 방향의 약속이 다릅니다. (B601-DM 은 축 방향까지 반대라 θ1 = 180° + q2, θ2 = −q3 − 180° 가 됩니다.)

단순화의 한계: 오프셋#

URDF 의 joint4 origin 은 xyz="0.228 -0.072746 0.0045" 입니다. 전완이 앞으로 22.8 cm 가는 동안 옆으로 7.3 cm 꺾여 있다는 뜻입니다(옆에서 보면 위쪽으로). 또 손목(joint4) 앞에는 손목 관절과 그리퍼가 더 붙어 있습니다. joint5·joint6 origin 의 x(0.087, 0.0365)와 그리퍼 0.16621 m 를 더하면, 손목을 펴 둔 상태(q4 = q5 = q6 = 0)에서 TCP 는 joint4 에서 전완 방향으로 0.2897 m 더 나가 있습니다.

이 두 가지를 모두 넣으면 옆모습 모델이 3D 계산과 정확히 일치합니다. 직접 확인해 봅시다.

python
import math

L1 = 0.236
FX = 0.228 + 0.087 + 0.0365 + 0.16621   # 팔꿈치 → TCP, 전완 방향 길이 (m)
FY = 0.072746                           # 전완의 옆 꺾임 (오프셋)
SHOULDER = (0.020, 0.145)               # 어깨 위치 (x, z)

def side_tcp(q2, q3, offset=True):
    """B601-RS 옆모습 TCP (x, z). q1 = q4 = q5 = q6 = 0 일 때만 성립"""
    t1 = math.radians(180 - q2)          # 상완 방향
    t12 = math.radians(q3 - q2)          # 전완 방향 = θ1 + θ2
    fy = FY if offset else 0.0
    ex = SHOULDER[0] + L1 * math.cos(t1)
    ez = SHOULDER[1] + L1 * math.sin(t1)
    x = ex + FX * math.cos(t12) - fy * math.sin(t12)
    z = ez + FX * math.sin(t12) + fy * math.cos(t12)
    return x, z

for q2, q3 in [(0, 0), (90, 90), (60, 90), (100, 60)]:
    x0, z0 = side_tcp(q2, q3, offset=False)
    x1, z1 = side_tcp(q2, q3)
    fx, fy_, fz = arm.fk([0, q2, q3, 0, 0, 0])
    print(f"q2={q2:>3}, q3={q3:>3} | 오프셋 무시 ({x0:.3f}, {z0:.3f}) "
          f"| 오프셋 포함 ({x1:.3f}, {z1:.3f}) | arm.fk ({fx:.3f}, {fz:.3f})")
text
q2=  0, q3=  0 | 오프셋 무시 (0.302, 0.145) | 오프셋 포함 (0.302, 0.218) | arm.fk (0.302, 0.218)
q2= 90, q3= 90 | 오프셋 무시 (0.538, 0.381) | 오프셋 포함 (0.538, 0.454) | arm.fk (0.538, 0.454)
q2= 60, q3= 90 | 오프셋 무시 (0.350, 0.608) | 오프셋 포함 (0.314, 0.671) | arm.fk (0.314, 0.671)
q2=100, q3= 60 | 오프셋 무시 (0.458, 0.045) | 오프셋 포함 (0.504, 0.100) | arm.fk (0.504, 0.100)

(이 코드는 RS 모델에서 실행하세요.) 오프셋을 무시하면 7 cm 가량 틀리고, 넣으면 소수 셋째 자리까지 맞습니다. 하지만 이 방법은 q1, q4, q5, q6 이 0 일 때만 쓸 수 있습니다. 손목이 꺾이거나 베이스가 돌면 평면 그림이 깨집니다. 일반적인 경우를 다루려면 다음 절의 변환 행렬이 필요합니다.

4×4 동차 변환 행렬#

3D 에서는 "어디에 있나(위치)"와 "어느 쪽을 보나(방향)"를 함께 다뤄야 합니다. 이 둘을 4×4 표 하나에 담은 것이 동차 변환 행렬(homogeneous transform) T 입니다.

T = r11r12r13px r21r22r23py r31r32r33pz 0001 ■ 회전 R (3×3) 새 좌표계의 x·y·z 축이 어디를 향하나 (열 하나 = 축 하나의 방향) ■ 위치 p (3×1) 새 좌표계의 원점이 어디에 있나 (m) ■ 0 0 0 1 곱셈을 편하게 하려고 붙인 고정 줄
왼쪽 위 3×3 은 회전 R(새 좌표계의 축 방향), 오른쪽 열은 위치 p, 맨 아래 줄은 계산 편의를 위한 고정 줄입니다.
  • R 의 열 하나 = 새 좌표계 축 하나의 방향. 첫 열이 새 x 축, 둘째 열이 새 y 축, 셋째 열이 새 z 축입니다.
  • p = 새 좌표계 원점의 위치. 손끝 위치를 알고 싶으면 마지막에 이 열만 읽으면 됩니다.
  • 변환 두 개를 곱하면 "A 에서 B 로, B 에서 C 로" 를 이어 붙인 "A 에서 C 로" 가 됩니다. 2관절 공식의 "누적"이 행렬에서는 곱셈입니다.

자주 쓰는 기본 행렬은 세 가지입니다.

python
import math

def trans(x, y, z):          # 이동만
    return [[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]]

def rot_z(a):                # z 축 둘레로 a 라디안 회전
    c, s = math.cos(a), math.sin(a)
    return [[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]

def mat_mul(A, B):           # 4×4 행렬 곱 (numpy 없이)
    return [[sum(A[i][k] * B[k][j] for k in range(4)) for j in range(4)]
            for i in range(4)]

# 2관절 팔을 행렬로: 회전 θ1 → 앞으로 L1 → 회전 θ2 → 앞으로 L2
T = mat_mul(rot_z(math.radians(30)), trans(0.236, 0, 0))
T = mat_mul(T, rot_z(math.radians(45)))
T = mat_mul(T, trans(0.228, 0, 0))
print(f"손끝 = ({T[0][3]:.3f}, {T[1][3]:.3f})")
text
손끝 = (0.263, 0.338)

앞 절의 fk_2link(30, 45) 와 같은 답입니다. 삼각함수 공식을 따로 유도하지 않아도, 회전과 이동을 순서대로 곱하기만 하면 같은 결과가 나온다는 것이 행렬의 힘입니다.

곱하는 순서가 중요합니다

행렬 곱은 순서를 바꾸면 결과가 달라집니다(A·B ≠ B·A). 로봇에서는 언제나 베이스에서 손끝 쪽으로, 왼쪽에서 오른쪽으로 곱합니다. "먼저 돌고 앞으로 가기"와 "먼저 앞으로 가고 돌기"는 다른 위치에 도착합니다.

URDF 의 origin 과 axis 를 행렬로#

URDF 의 각 <joint> 는 부모 링크에서 자식 링크로 가는 변환을 두 부분으로 적어 둡니다.

xml
<joint name="joint2" type="revolute">
  <origin xyz="0.020343 0.027237 0.07" rpy="-1.5708 0 0" />
  <parent link="link1" />
  <child link="link2" />
  <axis xyz="0 0 1" />
  <limit lower="0" upper="3.14" effort="36" velocity="50" />
</joint>

(공식 ReBot_Arm_RS.urdf 에서 joint2 부분만 옮긴 것)

  1. origin — 관절이 0° 일 때 자식 좌표계가 어디에(xyz, m), 어떤 방향으로(rpy, rad) 놓였나. 고정 값입니다.
  2. axis — 관절이 도는 축. 관절 각 q 만큼 그 축 둘레로 회전합니다. 움직이는 값입니다.

그래서 관절 하나의 변환은 T = origin(xyz, rpy) · 회전(axis, q) 입니다.

rpy → 회전 행렬#

rpy 는 roll(x 축), pitch(y 축), yaw(z 축) 세 각도입니다. URDF 규칙에서는 고정된 x 축으로 roll → 고정된 y 축으로 pitch → 고정된 z 축으로 yaw 순서로 돌리며, 행렬로는 R = Rz(yaw) · Ry(pitch) · Rx(roll) 입니다. joint2 의 rpy="-1.5708 0 0" 은 x 축으로 −90° 돌려, 수평으로 누워 있던 z 축을 옆(y 방향)으로 눕히는 것입니다. 그래서 어깨는 옆을 축으로 위아래로 끄덕입니다.

회전축 부호 — RS 와 DM 이 다른 곳#

reBot 의 모든 관절 axis 는 z 축이지만 부호가 다릅니다.

관절B601-RS axisB601-DM axis
joint1(0, 0, −1)(0, 0, 1)
joint2(0, 0, 1)(0, 0, −1)
joint3 ~ joint6(0, 0, −1)(0, 0, 1)

axis 가 (0, 0, −1) 이면 "z 축 둘레로 −q 만큼 회전"과 같습니다. 그래서 코드에서는 rot_z(부호 × q) 로 씁니다. 이 차이 때문에 같은 joint1 = +30° 라도 RS 는 TCP 가 −y 쪽(로봇 기준 오른쪽)으로, DM 은 +y 쪽(왼쪽)으로 돕니다. 9장에서 본 "joint2·joint3 의 부호가 반대"도 같은 이유입니다.

실습 — URDF 뷰어로 축 확인하기
  1. URDF 뷰어를 열고 RS 모델을 불러온 뒤 조인트 축 표시를 켜세요.
  2. joint1 슬라이더를 + 쪽으로 움직이며 팔이 위에서 봤을 때 어느 방향으로 도는지 보세요.
  3. DM 으로 바꿔 같은 실험을 하고 방향을 비교하세요. URDF 원문 보기에서 두 모델의 <axis> 줄도 찾아보세요.

사슬 곱으로 TCP 계산하기 — 순수 Python 구현#

이제 모든 조각이 모였습니다. base_link 에서 출발해 joint1 ~ joint6, 그리고 그리퍼 고정 관절까지 차례로 곱하면 TCP 의 위치와 방향이 나옵니다.

base link1 link2 link3 link4 link5 link6 T1 T2 T3 T4 T5 T6 Ttool gripper_end ★ T_TCP = T1 · T2 · T3 · T4 · T5 · T6 · Ttool Ti = origin(xyz, rpy) · Rz(±qi) Ttool = origin(xyz, rpy) (고정) 왼쪽(베이스)부터 차례로 곱합니다. 결과 행렬의 오른쪽 열(px, py, pz)이 손끝 위치입니다.
T_TCP = T1·T2·T3·T4·T5·T6·Ttool. 결과 행렬의 오른쪽 열이 손끝 위치입니다.

아래 표의 숫자는 공식 URDF 의 joint 값을 그대로 옮긴 것입니다. numpy 없이 리스트만으로 계산하고, 마지막에 arm.fk() 와 비교합니다.

python
import math

def mat_mul(A, B):
    return [[sum(A[i][k] * B[k][j] for k in range(4)) for j in range(4)]
            for i in range(4)]

def origin(xyz, rpy):
    """URDF origin(xyz, rpy) → 4×4 행렬. R = Rz(yaw)·Ry(pitch)·Rx(roll)"""
    r, p, y = rpy
    cr, sr = math.cos(r), math.sin(r)
    cp, sp = math.cos(p), math.sin(p)
    cy, sy = math.cos(y), math.sin(y)
    return [[cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr, xyz[0]],
            [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr, xyz[1]],
            [-sp,   cp*sr,            cp*cr,            xyz[2]],
            [0, 0, 0, 1]]

def rot_z(a):
    c, s = math.cos(a), math.sin(a)
    return [[c, -s, 0, 0], [s, c, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]

# (origin xyz, origin rpy, axis z 부호) — 공식 URDF 에서 옮김
CHAIN = {
  "RS": [((-0.00034283, -0.00098683, 0.075), (0, 0, 0), -1),
         ((0.020343, 0.027237, 0.07), (-1.5708, 0, 0), +1),
         ((-0.236, 0, 0), (0, 0, 0), -1),
         ((0.228, -0.072746, 0.0045), (0, 0, 0), -1),
         ((0.087, -0.048, -0.03075), (-1.5708, 0, 0), -1),
         ((0.0365, 0, 0.048), (0, 1.5708, 0), -1)],
  "DM": [((-8.416e-05, 0, 0.08465), (0, 0, 0), +1),
         ((0.020084, 0.031625, 0.05555), (-1.5708, 0, 0), -1),
         ((-0.264, 0, 0), (0, 0, 0), +1),
         ((0.2426, -0.054, -0.001625), (0, 0, 0), +1),
         ((0.078308, -0.0375, -0.03), (-1.5708, 0, 0), +1),
         ((0.028008, 0, 0.04), (0, 1.5708, 0), +1)],
}
TOOL = {"RS": ((0, 0, 0.16621), (3.1416, -1.5708, 0)),      # j_gripper_end
        "DM": ((0, 0, 0.15539), (0, -1.5708, 3.1415))}      # end_joint

def my_fk(q_deg, model):
    T = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
    for (xyz, rpy, sign), q in zip(CHAIN[model], q_deg):
        T = mat_mul(T, origin(xyz, rpy))               # 고정 부분
        T = mat_mul(T, rot_z(sign * math.radians(q)))  # 관절 회전
    T = mat_mul(T, origin(*TOOL[model]))               # 그리퍼(TCP)
    return T

tests = [[0, 0, 0, 0, 0, 0], [0, 90, 90, 0, 0, 0], [30, 60, 90, 0, 0, 0],
         [-20, 70, 110, 30, -40, 15]]
if arm.model == "DM":                     # DM 은 joint2·3 부호 반대
    tests = [[a, -b, -c, d, e, f] for a, b, c, d, e, f in tests]

for q in tests:
    T = my_fk(q, arm.model)
    mine = (T[0][3], T[1][3], T[2][3])
    ref = arm.fk(q)
    err = math.dist(mine, ref) * 1000
    print(f"{q} → 내 계산 ({mine[0]:.3f}, {mine[1]:.3f}, {mine[2]:.3f})"
          f"  arm.fk 와 차이 {err:.2f} mm")

RS 모델에서 실행하면 이렇게 나옵니다.

text
[0, 0, 0, 0, 0, 0] → 내 계산 (0.302, 0.000, 0.218)  arm.fk 와 차이 0.00 mm
[0, 90, 90, 0, 0, 0] → 내 계산 (0.538, 0.000, 0.454)  arm.fk 와 차이 0.00 mm
[30, 60, 90, 0, 0, 0] → 내 계산 (0.272, -0.157, 0.671)  arm.fk 와 차이 0.00 mm
[-20, 70, 110, 30, -40, 15] → 내 계산 (0.185, -0.071, 0.797)  arm.fk 와 차이 0.00 mm

손목까지 꺾은 마지막 자세도 정확히 맞습니다. 차이가 0.0x mm 정도 보인다면 시뮬레이터가 π 를 더 정밀하게 쓰는 등의 반올림 차이이니 걱정하지 않아도 됩니다. 크게 다르다면 축 부호와 도/라디안 변환을 먼저 의심하세요.

방향도 읽을 수 있습니다#

T 의 왼쪽 위 3×3 은 TCP 좌표계의 방향입니다. 앞 코드 아래에 이어 붙여 실행하세요(my_fk 를 그대로 씁니다). 이 강좌에서 그리퍼의 접근 방향(앞으로 내미는 방향)은 TCP 좌표계의 +X 축, 즉 첫 번째 열입니다.

python
T = my_fk([0, 0, 0, 0, 0, 0], arm.model)
approach = (T[0][0], T[1][0], T[2][0])
print("영점 자세 접근 방향:", [round(v, 3) for v in approach])
print("tcp_pose():", [round(v, 3) for v in arm.tcp_pose()])

영점 자세에서는 [1.0, 0.0, 0.0], 즉 그리퍼가 앞(+X) 을 향합니다. 11·12장에서 그리퍼를 아래(−Z) 로 향하게 할 때 바로 이 열이 (0, 0, −1) 이 되도록 맞춥니다.

실습 — DM 으로 검증하고, URDF 를 직접 읽기
  1. 시뮬레이터 모델을 DM 으로 바꾸고 위 코드를 다시 실행하세요. 영점 자세 TCP 가 약 (0.260, 0, 0.192) 로 나오고 arm.fk 와 차이가 0 이면 성공입니다.
  2. (도전) 표를 손으로 옮기는 대신 arm.urdf() 로 URDF 원문을 받아 자동으로 CHAIN 을 만들어 보세요. 아래 코드를 출발점으로 쓰세요.
python
import xml.etree.ElementTree as ET

root = ET.fromstring(arm.urdf())
for j in root.findall("joint"):
    o = j.find("origin")
    a = j.find("axis")
    xyz = [float(v) for v in o.get("xyz").split()]
    rpy = [float(v) for v in o.get("rpy").split()]
    axis = a.get("xyz") if a is not None else "-"
    print(j.get("name"), j.get("type"), xyz, rpy, "axis", axis)

출력에서 revolute 6개는 CHAIN 으로, link6 에 붙은 fixed 관절은 TOOL 로 쓰면 됩니다. 이렇게 만들면 모델이 바뀌어도 코드를 고칠 필요가 없습니다.

정리#

  • 정기구학(FK) = 관절 각도 → 손끝(TCP) 위치·방향. 답은 항상 하나입니다.
  • 막대 끝점은 (L·cosθ, L·sinθ), 2관절 팔은 x = L1·cosθ1 + L2·cos(θ1+θ2) — 각도는 누적됩니다.
  • reBot 을 옆에서 보면 상완 0.236 m, 전완 ≈ 0.228 m(RS) 의 2관절 팔이지만, 전완의 7.3 cm 오프셋과 손목·그리퍼 길이를 넣어야 정확하고, 그것도 q1·q4~q6 이 0 일 때만 맞습니다.
  • 4×4 동차 변환 = 회전 R + 위치 p. 변환을 곱하면 이어 붙여집니다.
  • URDF joint 하나 = origin(xyz, rpy) · rot_z(부호 × q). RS 와 DM 은 축 부호가 다릅니다.
  • base 부터 그리퍼까지 차례로 곱한 행렬의 오른쪽 열이 TCP 위치, 첫 열이 접근 방향입니다. arm.fk() 와 비교해 검증합니다.

확인 문제#

  1. 2관절 팔에서 θ1 = 90°, θ2 = 90° 일 때 손끝 (x, y) 는? (L1 = 0.236, L2 = 0.228)
  2. 전완 항에 cos(θ2) 대신 cos(θ1 + θ2) 를 쓰는 이유는?
  3. 4×4 변환 행렬에서 TCP 의 위치는 어디를 읽으면 되나요?
  4. RS 의 joint1 axis 가 (0, 0, −1) 이라는 것은 코드에서 어떻게 반영하나요?
  5. 옆모습 2관절 모델이 3D 계산과 맞지 않게 되는 경우를 하나 드세요.
정답
  1. (−0.228, 0.236). 상완이 위로 0.236, 전완은 바닥 기준 180° 라 뒤로 0.228.
  2. 관절은 부모 링크 기준으로 돌기 때문에, 전완이 바닥과 이루는 각은 앞 관절 각이 누적된 θ1 + θ2 입니다.
  3. 오른쪽 열의 위 세 칸 (T[0][3], T[1][3], T[2][3]).
  4. z 축 둘레로 −q 만큼 회전하는 것과 같으므로 rot_z(-1 × math.radians(q)) 처럼 부호를 곱합니다.
  5. joint1(베이스)을 돌리거나, joint4~6(손목)을 0 이 아닌 각도로 꺾었을 때. 또 전완 오프셋(7.3 cm)을 무시하면 손끝 높이가 틀립니다.