14손목 카메라와 시각화

카메라가 모방학습의 절반입니다#

정책(16장)은 사람처럼 눈으로 보고 손을 움직입니다. 관절 각도만으로는 "블록이 어디 있는지" 를 알 수 없으니, 카메라 영상이 입력의 절반을 차지합니다. 보통 두 종류를 함께 씁니다.

이 장에서는 reBot 용 손목 카메라 마운트 4종, 카메라를 URDF 에 붙이는 방법, 카메라 좌표와 로봇 좌표를 오가는 변환(내부·외부 파라미터, 핸드-아이)을 배웁니다.

손목 카메라 마운트 4종#

reBot 저장소의 hardware/camera-mounts 에는 B601-RS 와 B601-DM 이 함께 쓰는 손목 카메라 마운트 STEP 파일이 있습니다. README 는 대표 4종을 다음처럼 정리합니다.

카탈로그에는 이 밖에 D455f_Mount.step 도 있고, 손목이 아닌 고정 카메라용 스탠드(V1: 3D 프린팅 몸체 + 어댑터 4종, V2: 알루미늄 프로파일 조립체)도 있습니다. 카탈로그 README 는 "치수·체결 부품·출력 설정·호환 검증은 제공하지 않으니 출력 전에 CAD 형상을 실제 카메라와 대조하라" 고 적고 있습니다.

카메라 URDF 조립체#

마운트를 실제로 달았다면, 로봇 모델(URDF)에도 같은 카메라를 달아야 RViz·MuJoCo·좌표 변환이 맞습니다. Rebot_Arm_description/Camera 폴더가 이를 위한 4종 카메라 조립체 URDF 를 제공합니다.

Camera/urdf/
├── uvc32.urdf
├── d405.urdf
├── d435i.urdf
└── gemini2.urdf

D405 조립체의 링크 사슬은 다음과 같습니다(파일에서 확인한 이름).

gripper_end            ← 비어 있는 자리표시(placeholder) 루트
 └─ gripper_to_mount   (fixed) → camera_mount_link        ← 마운트(브래킷)
     └─ camera_joint   (fixed) → camera_bottom_screw_frame ← 카메라 바닥 나사 기준
         └─ camera_link
             ├─ camera_color_frame → camera_color_optical_frame
             ├─ camera_depth_frame → camera_depth_optical_frame
             └─ camera_infra1/2_frame → ..._optical_frame

RS 와 DM 에 붙이는 방법이 다릅니다#

네 URDF 는 모두 **빈 gripper_end 링크를 뿌리**로 삼습니다. README 의 안내는 이렇습니다.

DM 쪽 연결은 예를 들면 이런 모양입니다.

<!-- DM 용 예시: origin 값은 실제 장착 위치에 맞게 정해야 합니다 (0 은 자리표시) -->
<joint name="end_to_camera_root" type="fixed">
  <parent link="end_link"/>
  <child link="gripper_end"/>   <!-- 카메라 조립체의 루트 링크 -->
  <origin xyz="0 0 0" rpy="0 0 0"/>
</joint>

장착 파라미터 표#

README 의 장착 파라미터입니다(단위: 미터, 라디안). 브래킷은 조립체 루트(gripper_end) 기준, 카메라는 브래킷 링크 기준입니다.

읽는 법: 그리퍼 몸체 원점의 접근 방향이 +X 이므로, 브래킷 X 가 −0.12 m 라는 것은 마운트가 TCP 에서 손목 쪽으로 약 12 cm 뒤에 있다는 뜻입니다. 카메라 RPY 의 약 −1.05 rad(약 −60°) 기울기는 카메라가 그리퍼 끝을 비스듬히 내려다보도록 고개를 숙인 각도입니다. (UVC 는 CAD 에서 생성한 값이며 표에 별도로 있습니다.)

카메라 좌표계와 광학 프레임#

카메라 링크가 왜 camera_color_frame 과 camera_color_optical_frame 두 개나 있을까요? ROS 관례(REP 103) 때문입니다.

D405 URDF 의 두 프레임 사이 조인트에는 rpy="-1.5708 0 -1.5708" 이 들어 있습니다. 축 이름을 바꿔 붙이는 회전일 뿐 위치는 같습니다. 깊이 카메라가 주는 3D 점(X, Y, Z)은 광학 프레임 기준이라는 것만 기억하세요. Z 가 곧 "카메라에서 얼마나 멀리" 입니다.

내부 파라미터 — 픽셀과 3D 점#

카메라는 3D 세상을 2D 픽셀로 찍습니다. 가장 단순한 핀홀(바늘구멍) 모델에서 광학 프레임의 점 (X, Y, Z) 는 다음 픽셀 (u, v) 에 찍힙니다.

u = fx · X / Z + cx
v = fy · Y / Z + cy

• fx, fy — 초점 거리(픽셀 단위). 클수록 확대(좁은 화각)
• cx, cy — 주점. 보통 영상 중심 근처(640×480 이면 약 320, 240)

이 네 값(과 렌즈 왜곡 계수)을 내부 파라미터(intrinsics) 라고 합니다. RealSense·Orbbec 같은 깊이 카메라는 공장 캘리브레이션 값을 SDK 로 읽을 수 있고, 일반 UVC 카메라는 체커보드 등으로 직접 구합니다.

깊이 카메라의 진짜 힘은 거꾸로 가는 계산입니다. 픽셀 (u, v) 에서 깊이 Z 를 읽으면,

# 예시 값입니다 (실제 값은 카메라 SDK 나 캘리브레이션으로 구합니다)
fx, fy, cx, cy = 600.0, 600.0, 320.0, 240.0
u, v, Z = 400, 300, 0.25          # 픽셀 (400, 300) 의 깊이가 0.25 m

X = (u - cx) * Z / fx
Y = (v - cy) * Z / fy
print(f"광학 프레임 점: X={X:.3f}  Y={Y:.3f}  Z={Z:.3f} m")
# → X=0.033 (오른쪽)  Y=0.025 (아래)  Z=0.250 (앞)

외부 파라미터와 핸드-아이#

이제 이 점을 로봇 베이스 좌표로 바꿔야 팔을 그곳으로 보낼 수 있습니다. 손목 카메라라면 변환이 사슬로 이어집니다.

p_base = T_base←tcp(q) · T_tcp←cam · p_cam
          ───────────    ──────────
          정기구학(10장)   외부 파라미터(카메라가 손목 어디에 붙었나)

• T_base←tcp(q) 는 관절각 q 로 매 순간 계산되는 정기구학입니다(10장).
• T_tcp←cam 은 카메라가 그리퍼에 어떻게 붙어 있는지 — 외부 파라미터(extrinsics) 입니다. 손목 카메라는 팔과 함께 움직이므로 이 값이 고정입니다. 이것이 eye-in-hand 의 특징입니다.

외부 파라미터는 두 가지 방법으로 얻습니다.

1. URDF 값 — 위 장착 파라미터 표처럼 CAD 에서 나온 이론값. 시각화·대략적 계산에 충분합니다.
2. 핸드-아이 캘리브레이션 — 실제 조립 오차까지 반영한 측정값. Seeed 의 비전 그리핑 데모는 ArUco 마커를 놓고 팔을 여러 자세로 움직이며 마커를 찍은 뒤, TSAI 방식 Eye-in-Hand 로 이 변환을 계산합니다(scripts/collect_handeye_eih.py, 자동 모드 50개 자세, 최소 5개·권장 15개 이상 샘플). 결과는 intrinsics.npz, hand_eye.npz 로 저장됩니다. 자세한 실습은 21장에서 합니다.

import math

arm.move_to(0.40, 0.0, 0.10, t=1.5, down=True)   # 그리퍼를 아래로
x, y, z, r, p, yw = arm.tcp_pose()                 # 미터·도
print("TCP:", round(x, 3), round(y, 3), round(z, 3), " rpy:", round(r), round(p), round(yw))

def rpy_to_R(roll, pitch, yaw):                    # URDF 관례 R = Rz·Ry·Rx (도 입력)
    a, b, c = [math.radians(v) for v in (roll, pitch, yaw)]
    ca, sa, cb, sb, cc, sc = math.cos(a), math.sin(a), math.cos(b), math.sin(b), math.cos(c), math.sin(c)
    return [[cc*cb, cc*sb*sa - sc*ca, cc*sb*ca + sc*sa],
            [sc*cb, sc*sb*sa + cc*ca, sc*sb*ca - cc*sa],
            [-sb,   cb*sa,            cb*ca]]

R = rpy_to_R(r, p, yw)
approach = [R[0][0], R[1][0], R[2][0]]             # TCP 의 +X 축 = 접근 방향
print("접근 방향:", [round(v, 2) for v in approach])  # 아래(0, 0, -1)에 가까워야 정상

d = 0.07                                            # 카메라가 앞쪽 0.07 m 에서 물체를 봤다면
target = [x + d*approach[0], y + d*approach[1], z + d*approach[2]]
print("물체의 베이스 좌표:", [round(v, 3) for v in target])
arm.move_to(target[0], target[1], target[2] + 0.05, t=1.5, down=True)  # 5 cm 위로 접근

시뮬레이터 카메라 패널과 LeRobot 카메라 설정#

MuJoCo 기반 온라인 데모(README 소개)에서도 B601-RS 의 D405 손목 카메라와 상단 카메라 영상을 브라우저로 볼 수 있습니다.

실물에서는 LeRobot 의 --robot.cameras 에 카메라를 적습니다. Seeed 위키 기준으로 일반 USB 카메라는 opencv 타입, 깊이 카메라는 Seeed LeRobot 포크의 DepthCameraSupport 브랜치에서 realsense_d405_color · realsense_d405_depth · orbbec_color 같은 타입을 씁니다. 카메라 찾기는 다음 명령입니다.

lerobot-find-cameras opencv      # 일반 USB 카메라 (인덱스 확인)
lerobot-find-cameras realsense   # RealSense (일련번호 확인)
lerobot-find-cameras orbbec      # Orbbec

전체 명령은 15장에서 녹화와 함께 다룹니다.

정리#

• 손목 카메라(eye-in-hand)는 근접 장면을, 고정 카메라(eye-to-hand)는 전체 배치를 봅니다. 둘을 함께 쓰는 것이 기본입니다.
• 공식 손목 마운트는 RS·DM 공용 4종: UVC32, D405/Gemini 305, D435i/Gemini 2(공용 마운트) — STEP 파일로 제공됩니다.
• 카메라 URDF 4종은 빈 gripper_end 루트를 가집니다. RS 는 자리표시를 지우고 기존 gripper_end 에 합치고, DM 은 end_link 에 고정 조인트(실제 장착 변환)로 연결합니다. 한 번에 하나만 씁니다.
• 내부 파라미터(fx, fy, cx, cy)는 픽셀 ↔ 3D 점, 외부 파라미터는 카메라 ↔ 그리퍼 관계입니다. p_base = T_base←tcp(q) · T_tcp←cam · p_cam.
• 정확한 외부 파라미터는 핸드-아이 캘리브레이션(ArUco + TSAI Eye-in-Hand)으로 구하며, 마운트를 다시 달면 다시 해야 합니다.

확인 문제#

1. 손목 카메라와 고정 카메라가 각각 잘 보는 것과 약한 것을 하나씩 적어 보세요.
2. D435i 와 Gemini 2 손목 카메라는 어떤 마운트 파일을 공유하나요?
3. 카메라 URDF 조립체를 B601-DM 에 붙일 때 RS 와 달리 무엇을 해야 하나요?
4. fx = fy = 600, cx = 320, cy = 240 인 카메라에서 픽셀 (320, 240) 의 깊이가 0.3 m 입니다. 광학 프레임 좌표는?
5. eye-in-hand 구성에서 T_tcp←cam 이 "고정값" 인 이유와, 이 값이 틀어지는 대표적인 상황은?