카메라를 보고 물체 집기 프로젝트

프로젝트의 목표: 보는 로봇팔 만들기

지금까지 우리는 로봇팔을 움직이는 법과 카메라로 물체를 인식하는 법을 따로 배웠다. 카메라는 이미지를 숫자로 바꾸고, 객체 검출 모델은 그 안에서 컵이나 상자 같은 대상을 찾아낸다. 깊이 카메라는 픽셀 하나를 3차원 위치로 바꿔 주고, 좌표 변환은 그 위치를 로봇팔이 이해할 수 있는 기준 좌표로 옮긴다. MoveIt2는 그 목표 위치까지 로봇팔이 어떻게 이동할지 계획한다. 마지막으로 Pick and Place 시퀀스는 접근하고, 집고, 들어 올리고, 옮기고, 놓는 행동을 순서대로 실행한다.

이번 장의 목표는 이 조각들을 하나의 시스템으로 연결하는 것이다. 이제 로봇팔은 단순히 "좌표 x, y, z로 이동하라"는 명령만 받지 않는다. 카메라를 보고, 책상 위 물체를 찾고, 물체의 위치를 계산한 뒤, 스스로 집기 동작을 시도한다.

우리가 만들 프로젝트의 흐름은 다음과 같다.

1. 카메라에서 RGB 이미지와 깊이 이미지를 받는다.

2. 객체 인식 모델로 목표 물체를 찾는다.

3. 픽셀 위치와 깊이값을 이용해 카메라 기준 3D 좌표를 계산한다.

4. 카메라 좌표를 로봇 베이스 좌표로 변환한다.

5. 물체 위치를 바탕으로 집기 후보 자세를 만든다.

6. MoveIt2로 접근, 집기, 들어 올리기, 이동, 놓기 경로를 계획한다.

7. 로봇팔과 그리퍼를 실행한다.

8. 카메라로 결과를 다시 확인하고 성공 또는 실패를 판정한다.

글로 쓰면 길어 보이지만, 핵심은 간단하다. 카메라가 목표를 찾고, 좌표 변환이 위치를 번역하고, MoveIt2가 팔의 움직임을 만들고, 카메라가 다시 결과를 확인한다. 이것이 시각 기반 로봇 조작의 기본 구조다.

전체 시스템을 한눈에 보기

이번 프로젝트를 하나의 공장 라인처럼 상상해 보자. 카메라 노드는 원재료인 이미지를 계속 공급한다. 인식 노드는 이미지에서 물체 후보를 찾아낸다. 위치 추정 노드는 물체가 실제 공간 어디에 있는지 계산한다. 집기 계획 노드는 "어느 방향에서 어떻게 잡을지"를 정한다. 실행 노드는 로봇팔을 움직인다. 검증 노드는 작업이 성공했는지 확인한다.

구조를 단순화하면 다음과 같다.

[RGB-D Camera]
      ↓
[Object Detection / Segmentation]
      ↓
[3D Position Estimation]
      ↓
[Camera → Robot Base Transform]
      ↓
[Grasp Candidate Generation]
      ↓
[MoveIt2 Planning]
      ↓
[Pick and Place Execution]
      ↓
[Visual Verification]
      ↓
[Success or Retry]

이 구조에서 중요한 점은 모든 모듈이 완벽할 필요는 없다는 것이다. 처음부터 최신 객체 검출 모델, 정교한 세그멘테이션, 고급 집기 정책을 모두 넣으려 하면 프로젝트가 너무 커진다. 처음에는 색상 기반 검출이나 간단한 바운딩 박스 중심점으로 시작해도 좋다. 중요한 것은 입력에서 실행까지 데이터가 끊기지 않고 흐르는 경험을 하는 것이다.

프로젝트를 다음과 같은 모듈로 나누면 디버깅이 쉬워진다.

이처럼 나누면 어느 단계에서 문제가 생겼는지 바로 찾을 수 있다. 물체가 검출되지 않는다면 perception_node를 보면 되고, 로봇팔이 엉뚱한 위치로 간다면 pose_estimation_node나 transform_node를 확인하면 된다. 계획은 되는데 실행 중 충돌이 난다면 grasp_planner나 MoveIt2 설정을 점검해야 한다.

프로젝트 시나리오 정하기

통합 프로젝트에서는 너무 어려운 작업부터 시작하지 않는 것이 좋다. 예를 들어 "책상 위 아무 물건이나 적절히 정리해 줘"는 사람에게는 쉬워 보이지만 로봇에게는 복합 문제다. 물체 인식, 물체 종류 판단, 잡기 방식 선택, 놓을 위치 결정, 장애물 회피, 실패 복구가 모두 필요하다.

첫 프로젝트는 다음 정도가 적당하다.

책상 위에 있는 빨간 블록을 집어서 오른쪽 바구니에 놓는다.

또는 다음처럼 조금 더 일반화할 수 있다.

카메라가 인식한 컵을 집어서 미리 정해진 place 위치에 놓는다.

여기에는 우리가 배운 핵심 기술이 모두 들어 있다. 물체를 찾아야 하고, 3D 위치를 계산해야 하며, 로봇팔이 접근 가능한 자세를 만들어야 한다. 그리퍼로 물체를 집고, 들어 올리고, 정해진 위치로 이동해 내려놓아야 한다. 마지막으로 카메라를 다시 보고 물체가 원래 자리에서 사라졌는지, 또는 목표 영역에 나타났는지 확인한다.

처음에는 물체도 단순한 것이 좋다. 투명 컵이나 반짝이는 금속 물체는 깊이 카메라가 어려워할 수 있다. 너무 납작한 종이, 너무 작은 나사, 쉽게 굴러가는 공도 초반 실습에는 적합하지 않다. 직육면체 블록, 무광 컵, 작은 상자처럼 형태가 안정적이고 집기 쉬운 물체를 선택하자.

카메라 입력 받기

첫 단계는 카메라에서 RGB 이미지와 깊이 이미지를 안정적으로 받는 것이다. 로봇 프로젝트에서 "카메라가 보인다"는 말은 단순히 화면에 영상이 나온다는 뜻이 아니다. 다음 조건이 함께 만족되어야 한다.

• RGB 이미지와 깊이 이미지의 해상도, 프레임 시간이 맞아야 한다.

• 카메라 내부 파라미터가 있어야 픽셀을 3D 좌표로 바꿀 수 있다.

• 카메라 프레임 이름이 TF 트리에서 일관되게 관리되어야 한다.

RGB-D 카메라를 쓰면 보통 RGB 이미지 토픽, 깊이 이미지 토픽, 카메라 정보 토픽이 함께 나온다. RGB 이미지는 물체가 무엇인지 찾는 데 사용하고, 깊이 이미지는 그 물체가 얼마나 떨어져 있는지 계산하는 데 사용한다. 카메라 정보에는 초점거리와 중심점 같은 내부 파라미터가 들어 있다.

픽셀 좌표를 3D 좌표로 바꾸려면 다음 값이 필요하다.

u, v : 이미지에서 물체 중심 픽셀 좌표
Z    : 해당 픽셀의 깊이값
fx   : x축 초점거리
fy   : y축 초점거리
cx   : 이미지 중심 x좌표
cy   : 이미지 중심 y좌표

카메라 좌표계에서 3D 위치는 다음처럼 계산할 수 있다.

X = (u - cx) * Z / fx
Y = (v - cy) * Z / fy
Z = Z

여기서 주의할 점은 깊이값의 단위다. 어떤 카메라는 깊이를 밀리미터 단위로 주고, 어떤 설정에서는 미터 단위로 준다. 로봇 제어에서는 보통 미터 단위를 사용한다. 만약 깊이값 650을 650미터로 해석하면 로봇팔은 지구 반대편을 향해 팔을 뻗으려는 셈이 된다. 실제로는 650밀리미터, 즉 0.65미터일 가능성이 높다.

처음 디버깅할 때는 이미지를 보며 마우스로 한 점을 찍고 그 픽셀의 깊이값을 출력해 보자. 책상 위 물체가 카메라에서 50cm 정도 떨어져 있다면 깊이값이 0.5m 근처로 나와야 한다. 이 간단한 확인만으로도 많은 오류를 잡을 수 있다.

객체를 인식하고 대상 물체 선택하기

카메라 입력이 준비되면 다음은 물체 인식이다. 인식 방법은 프로젝트 수준에 따라 다르게 선택할 수 있다.

처음에는 색상 기반 검출이 가장 쉽다. 예를 들어 빨간 블록을 찾는다면 RGB 이미지를 HSV 색상 공간으로 바꾸고 빨간색 범위만 마스크로 추출하면 된다. 이 방식은 조명이 바뀌거나 비슷한 색 물체가 있으면 흔들리지만, 통합 흐름을 익히기에는 훌륭하다.

그다음 단계는 객체 검출 모델을 사용하는 것이다. YOLO 계열 모델처럼 이미지에서 물체 클래스와 바운딩 박스를 반환하는 모델을 쓰면 "컵", "병", "상자" 같은 의미 있는 물체를 찾을 수 있다. 바운딩 박스 중심점을 물체 중심 후보로 삼고, 그 주변 깊이값을 사용해 3D 위치를 계산한다.

더 정교하게 하려면 세그멘테이션 마스크를 사용할 수 있다. 마스크는 물체가 차지하는 픽셀 영역을 알려 주므로, 바운딩 박스보다 물체의 실제 모양을 잘 반영한다. 특히 그리퍼가 집을 수 있는 영역을 찾을 때 도움이 된다. 바운딩 박스 중심은 빈 공간이나 손잡이 구멍을 가리킬 수 있지만, 마스크는 실제 물체 픽셀만 골라낼 수 있기 때문이다.

이 장에서는 구조를 이해하기 위해 검출 결과를 다음 형태로 통일해 보자.

@dataclass
class DetectedObject:
    label: str
    score: float
    bbox: tuple[int, int, int, int]  # x1, y1, x2, y2
    mask: Optional[np.ndarray] = None

대상 물체 선택은 생각보다 중요하다. 이미지에 컵이 두 개 있다면 어느 컵을 집어야 할까? 사용자가 "컵을 집어"라고만 말했을 때는 가장 가까운 컵을 선택할 수도 있고, 검출 점수가 가장 높은 컵을 선택할 수도 있다. 이번 프로젝트에서는 규칙을 단순하게 정하자.

def select_target(objects, target_label):
    candidates = [obj for obj in objects if obj.label == target_label]
    if not candidates:
        return None
    return max(candidates, key=lambda obj: obj.score)

이 코드는 목표 label과 일치하는 물체 중 검출 점수가 가장 높은 것을 선택한다. 나중에 자연어 명령을 연결하면 "빨간 컵", "왼쪽 상자", "가장 가까운 병"처럼 조건이 더 복잡해진다. 하지만 기본 원리는 같다. 인식 결과 중에서 현재 작업 목표에 맞는 물체 하나를 고르는 것이다.

픽셀에서 3D 위치로

물체를 선택했다면 이제 픽셀 위치를 실제 공간 위치로 바꿔야 한다. 가장 단순한 방법은 바운딩 박스 중심 픽셀을 사용하는 것이다.

def bbox_center(bbox):
    x1, y1, x2, y2 = bbox
    u = int((x1 + x2) / 2)
    v = int((y1 + y2) / 2)
    return u, v

하지만 현실에서는 중심 픽셀의 깊이값이 비어 있거나 튈 수 있다. 깊이 카메라는 반사, 그림자, 물체 경계에서 오류를 자주 낸다. 그래서 중심점 하나만 믿기보다 주변 작은 영역의 깊이값을 함께 보는 것이 안전하다.

def robust_depth(depth_image, u, v, window=5):
    h, w = depth_image.shape[:2]
    x1 = max(0, u - window)
    x2 = min(w, u + window + 1)
    y1 = max(0, v - window)
    y2 = min(h, v + window + 1)

    patch = depth_image[y1:y2, x1:x2]
    valid = patch[np.isfinite(patch)]
    valid = valid[valid > 0]

    if len(valid) == 0:
        return None

    return float(np.median(valid))

여기서는 평균이 아니라 중앙값을 사용했다. 평균은 하나의 이상한 값에 크게 흔들릴 수 있다. 반면 중앙값은 주변 값 중 가운데 값을 선택하므로 깊이 노이즈에 조금 더 강하다.

마스크가 있다면 더 좋다. 바운딩 박스 전체가 아니라 물체 마스크 안쪽 픽셀들만 모아서 깊이값을 계산할 수 있다. 이때도 전체 픽셀을 무작정 평균내기보다 유효한 깊이값을 고르고 중앙값을 쓰는 편이 안정적이다.

def mask_center_and_depth(mask, depth_image):
    ys, xs = np.where(mask > 0)
    if len(xs) == 0:
        return None

    u = int(np.median(xs))
    v = int(np.median(ys))

    depths = depth_image[ys, xs]
    valid = depths[np.isfinite(depths)]
    valid = valid[valid > 0]

    if len(valid) == 0:
        return None

    z = float(np.median(valid))
    return u, v, z

그다음 카메라 내부 파라미터를 사용해 3D 좌표를 만든다.

def pixel_to_camera_point(u, v, z, fx, fy, cx, cy):
    x = (u - cx) * z / fx
    y = (v - cy) * z / fy
    return np.array([x, y, z], dtype=float)

이 함수가 반환하는 좌표는 아직 로봇팔 기준이 아니다. 카메라가 바라보는 방향, 카메라의 설치 각도, 카메라 좌표계의 축 방향을 기준으로 한 좌표다. 로봇팔에게는 "카메라 앞 0.6m"보다 "로봇 베이스 기준 x=0.35m, y=0.10m, z=0.08m"가 필요하다. 그래서 다음 단계가 좌표 변환이다.

카메라 좌표를 로봇 베이스 좌표로 변환하기

앞 장에서 다룬 것처럼 로봇 시스템에는 여러 좌표계가 동시에 존재한다. 카메라 좌표계, 로봇 베이스 좌표계, 엔드 이펙터 좌표계, 그리퍼 좌표계가 모두 따로 있다. 로봇팔을 움직일 때 기준이 되는 좌표계는 보통 base_link 또는 world다. 따라서 카메라에서 계산한 점을 로봇 베이스 기준으로 변환해야 한다.

ROS2 환경에서는 TF를 사용해 이 변환을 관리하는 것이 일반적이다. 예를 들어 카메라 광학 프레임이 camera_color_optical_frame이고 로봇 베이스가 base_link라면, 우리는 다음 변환이 필요하다.

camera_color_optical_frame → base_link

실제 코드에서는 TF 버퍼에서 변환을 조회한 뒤, 카메라 기준 3D 점에 변환을 적용한다.

def transform_point(tf_buffer, point_cam, source_frame, target_frame):
    transform = tf_buffer.lookup_transform(
        target_frame,
        source_frame,
        rclpy.time.Time()
    )

    stamped = PointStamped()
    stamped.header.frame_id = source_frame
    stamped.point.x = float(point_cam[0])
    stamped.point.y = float(point_cam[1])
    stamped.point.z = float(point_cam[2])

    point_base = do_transform_point(stamped, transform)
    return np.array([
        point_base.point.x,
        point_base.point.y,
        point_base.point.z
    ], dtype=float)

이때 가장 흔한 실수는 frame 이름이 맞지 않는 것이다. 카메라 드라이버가 제공하는 프레임 이름과 URDF 또는 static transform에서 정의한 프레임 이름이 다르면 변환을 찾을 수 없다. 두 번째 실수는 optical frame의 축 방향을 착각하는 것이다. 카메라 optical frame은 보통 z축이 카메라가 바라보는 방향이고, x축은 이미지 오른쪽, y축은 이미지 아래쪽이다. 로봇 베이스 좌표계와 방향이 다르므로 손으로 축을 맞추려 하지 말고 TF를 통해 일관되게 처리하는 것이 좋다.

변환이 잘되었는지 확인하는 가장 좋은 방법은 시각화다. RViz에서 카메라 이미지, 포인트 클라우드, TF, 로봇 모델, 계산된 물체 위치 마커를 함께 띄워 보자. 물체 마커가 실제 책상 위 물체와 비슷한 위치에 떠 있다면 절반은 성공이다. 반대로 마커가 로봇 뒤쪽이나 바닥 아래에 나타난다면 좌표 변환, 단위, 축 방향 중 하나가 잘못되었을 가능성이 높다.

집기 후보 자세 만들기

이제 로봇 베이스 기준 물체 위치를 얻었다. 하지만 물체 위치 하나만으로는 로봇팔을 움직일 수 없다. 그리퍼는 어느 방향에서 접근해야 할까? 물체 바로 중심으로 가면 충돌하지 않을까? 집기 전에 그리퍼를 얼마나 벌려야 할까? 물체를 집은 뒤 얼마나 들어 올려야 할까?

우리는 앞에서 Pick and Place를 다음 세 자세로 나누었다.

pre_grasp_pose  : 물체 위 또는 앞쪽의 접근 준비 자세
grasp_pose      : 실제로 그리퍼가 물체를 잡는 자세
post_grasp_pose : 물체를 잡은 뒤 들어 올린 자세

이번 프로젝트에서는 위에서 아래로 내려와 집는 top-down grasp를 기본으로 하자. 책상 위 블록이나 컵을 집는 가장 단순한 방식이다. 물체의 중심 위치가 object_pos = [x, y, z]라면 grasp pose는 물체 중심보다 약간 위에 둔다. 그리퍼 손가락의 길이와 물체 높이에 따라 다르지만, 처음에는 보수적으로 잡는다.

def make_grasp_poses(object_pos):
    x, y, z = object_pos

    pre_grasp = Pose()
    pre_grasp.position.x = x
    pre_grasp.position.y = y
    pre_grasp.position.z = z + 0.15

    grasp = Pose()
    grasp.position.x = x
    grasp.position.y = y
    grasp.position.z = z + 0.03

    post_grasp = Pose()
    post_grasp.position.x = x
    post_grasp.position.y = y
    post_grasp.position.z = z + 0.20

    # 실제 코드에서는 그리퍼가 아래를 향하도록 orientation을 설정한다.
    set_top_down_orientation(pre_grasp)
    set_top_down_orientation(grasp)
    set_top_down_orientation(post_grasp)

    return pre_grasp, grasp, post_grasp

여기서 z + 0.03 같은 값은 모든 로봇에 맞는 정답이 아니다. 그리퍼의 손가락 길이, 물체 높이, 카메라가 측정한 위치가 물체의 윗면인지 중심인지 바닥 근처인지에 따라 달라진다. 실제 프로젝트에서는 이 값을 실험적으로 조정해야 한다. 처음에는 물체보다 살짝 높은 위치로 가게 해서 충돌을 피하고, 조금씩 낮추며 잡히는 지점을 찾는 것이 안전하다.

이전 장에서 중단되었던 중요한 질문도 여기서 다시 이어진다. 집기 후보가 여러 개라면 어떤 것을 선택해야 할까? 예를 들어 같은 물체를 위에서 잡을 수도 있고, 옆에서 잡을 수도 있다. 위에서 잡는 경우에도 중심보다 약간 왼쪽을 잡는 편이 더 안정적일 수 있다. 이때는 후보마다 점수를 매겨 선택한다.

간단한 점수 기준은 다음과 같다.

• 로봇팔이 도달 가능한 자세인가?

• MoveIt2가 충돌 없는 경로를 찾을 수 있는가?

• 그리퍼가 물체의 실제 영역 안쪽을 잡는가?

• 책상, 벽, 다른 물체와 충분히 떨어져 있는가?

• 이전 실패한 후보와 너무 비슷하지 않은가?

이를 코드 구조로 표현하면 다음과 같다.

def score_grasp_candidate(candidate, planning_result, object_mask=None):
    score = 0.0

    if planning_result.reachable:
        score += 1.0
    if planning_result.collision_free:
        score += 1.0
    if candidate.clearance > 0.03:
        score += 0.5
    if candidate.not_tried_before:
        score += 0.5

    return score

처음 구현에서는 후보를 하나만 만들어도 된다. 하지만 실패를 다루려면 후보를 여러 개 만들고, 실패한 후보를 제외한 뒤 다음 후보를 시도하는 구조가 필요하다. 이것이 나중에 LLM 에이전트의 재계획 인터페이스와도 연결된다. 상위 에이전트는 "같은 물체를 다른 방향에서 다시 잡아 봐"라고 명령할 수 있고, 하위 로봇 스킬은 후보 목록에서 새로운 자세를 선택해 실행할 수 있다.

MoveIt2로 접근, 집기, 이동 계획하기

집기 자세가 준비되면 MoveIt2에 목표 자세를 넘겨 경로를 계획한다. 여기서 중요한 것은 Pick and Place를 하나의 거대한 동작으로 보지 않는 것이다. 로봇팔은 다음처럼 짧은 단계들을 순서대로 실행해야 한다.

1. 홈 자세 또는 대기 자세로 이동
2. 그리퍼 열기
3. pre_grasp_pose로 이동
4. grasp_pose로 천천히 접근
5. 그리퍼 닫기
6. post_grasp_pose로 들어 올리기
7. place_pre_pose로 이동
8. place_pose로 내려가기
9. 그리퍼 열기
10. retreat_pose로 빠져나오기

각 단계는 성공 또는 실패를 반환해야 한다. "어딘가에서 실패했다"가 아니라 "pre_grasp 계획 실패", "grasp 접근 중 충돌 위험", "gripper close 후 물체 없음"처럼 원인을 알 수 있어야 한다. 그래야 다음 행동을 정할 수 있다.

구조는 다음처럼 만들 수 있다.

class PickPlaceExecutor:
    def __init__(self, moveit, gripper):
        self.moveit = moveit
        self.gripper = gripper

    def execute(self, grasp_plan, place_pose):
        result = self.move_to(grasp_plan.pre_grasp, stage="pre_grasp")
        if not result.success:
            return result

        result = self.open_gripper()
        if not result.success:
            return result

        result = self.move_to(grasp_plan.grasp, stage="grasp")
        if not result.success:
            return result

        result = self.close_gripper()
        if not result.success:
            return result

        result = self.check_grasp()
        if not result.success:
            return result

        result = self.move_to(grasp_plan.post_grasp, stage="lift")
        if not result.success:
            return result

        result = self.move_to(place_pose.pre_place, stage="pre_place")
        if not result.success:
            return result

        result = self.move_to(place_pose.place, stage="place")
        if not result.success:
            return result

        result = self.open_gripper()
        if not result.success:
            return result

        result = self.move_to(place_pose.retreat, stage="retreat")
        return result

실제 MoveIt2 코드는 사용하는 언어와 라이브러리 구성에 따라 달라진다. Python으로 간단히 래핑할 수도 있고, C++의 MoveGroupInterface를 사용할 수도 있다. 여기서 우리가 집중해야 할 것은 특정 API 이름보다 구조다. 목표 자세를 만들고, 계획하고, 실행하고, 결과를 확인하는 흐름이 핵심이다.

move_to() 함수는 내부적으로 다음 일을 한다.

• 목표 pose를 MoveIt2에 설정한다.

• 경로 계획을 요청한다.

• 계획 결과가 없으면 실패를 반환한다.

• 계획된 경로가 안전 조건을 만족하는지 확인한다.

• 실행하고 완료 상태를 확인한다.

그리퍼 제어도 마찬가지다. 단순한 전동 그리퍼라면 열기와 닫기 명령만 있을 수 있다. 조금 더 좋은 그리퍼라면 현재 벌어진 폭, 힘, 물체 감지 상태를 알려 준다. 이런 정보가 있으면 "정말 물체를 잡았는가?"를 더 잘 판단할 수 있다.

통합 노드 만들기

이제 전체 흐름을 하나의 오케스트라처럼 지휘하는 노드가 필요하다. 이름을 vision_pick_node라고 해 보자. 이 노드는 직접 모든 계산을 다 할 수도 있고, 여러 서비스를 호출하는 조정자 역할만 할 수도 있다. 처음 프로젝트에서는 하나의 노드 안에 주요 흐름을 넣어도 된다. 나중에 시스템이 커지면 인식, 좌표 변환, 실행을 각각 분리하면 된다.

통합 노드의 의사코드는 다음과 같다.

class VisionPickNode(Node):
    def __init__(self):
        super().__init__("vision_pick_node")
        self.latest_rgb = None
        self.latest_depth = None
        self.camera_info = None

        self.detector = ObjectDetector()
        self.pose_estimator = PoseEstimator()
        self.grasp_planner = GraspPlanner()
        self.executor = PickPlaceExecutor()
        self.verifier = VisualVerifier()

    def run_once(self, target_label, place_name):
        image = self.wait_for_rgb()
        depth = self.wait_for_depth()
        camera_info = self.wait_for_camera_info()

        objects = self.detector.detect(image)
        target = select_target(objects, target_label)
        if target is None:
            return SkillResult(False, "detect", "target_not_found")

        point_cam = self.pose_estimator.estimate(target, depth, camera_info)
        if point_cam is None:
            return SkillResult(False, "pose_estimation", "invalid_depth")

        point_base = self.transform_to_base(point_cam)
        if not self.is_inside_workspace(point_base):
            return SkillResult(False, "safety", "out_of_workspace")

        grasp_candidates = self.grasp_planner.generate(point_base, target)
        place_pose = self.get_place_pose(place_name)

        for candidate in grasp_candidates:
            plan = self.executor.plan(candidate, place_pose)
            if not plan.success:
                continue

            result = self.executor.execute_plan(plan)
            if not result.success:
                self.grasp_planner.mark_failed(candidate, result.reason)
                continue

            verify = self.verifier.check(target_label, place_name)
            if verify.success:
                return SkillResult(True, "done", "success")

            self.grasp_planner.mark_failed(candidate, "visual_verification_failed")

        return SkillResult(False, "retry", "all_candidates_failed")

이 코드에서 눈여겨볼 부분은 실패를 즉시 끝으로 보지 않는다는 점이다. 첫 번째 후보가 실패하면 다음 후보를 시도한다. 깊이값이 이상하면 pose estimation 단계에서 멈춘다. 작업 공간 밖의 좌표가 나오면 안전 단계에서 차단한다. 실행 후 카메라 검증이 실패하면 다시 시도할 수 있다.

좋은 로봇 시스템은 성공할 때 멋진 시스템이 아니라, 실패했을 때 무너져 내리지 않는 시스템이다. 물체를 놓쳤다면 왜 놓쳤는지 기록하고, 다른 후보를 고르고, 필요하면 사용자에게 "물체가 너무 가장자리에 있어서 잡기 어렵습니다"라고 알려 줄 수 있어야 한다.

실행 결과를 카메라로 다시 확인하기

로봇팔이 그리퍼를 닫았다고 해서 물체를 잡은 것은 아니다. 그리퍼가 빈 공간에서 닫혔을 수도 있고, 물체를 살짝 밀어냈을 수도 있다. 들어 올리는 순간 물체가 미끄러져 떨어질 수도 있다. 그래서 카메라 기반 검증이 필요하다.

검증 방법은 작업 목표에 따라 달라진다. 가장 단순한 방법은 "원래 위치에서 물체가 사라졌는가?"를 확인하는 것이다. 집기 전 물체의 바운딩 박스 영역을 기억해 두고, 집기 후 같은 영역에 목표 물체가 여전히 보이는지 확인한다. 여전히 보이면 집기에 실패했을 가능성이 높다.

조금 더 나은 방법은 "목표 놓기 영역에 물체가 나타났는가?"를 확인하는 것이다. 예를 들어 오른쪽 바구니 영역을 이미지에서 미리 정의해 두고, place 후 그 영역 안에서 목표 물체가 검출되는지 확인한다. 또는 목표 위치 주변의 깊이 변화가 생겼는지 볼 수도 있다.

검증은 다음 두 단계로 나눌 수 있다.

집기 검증: 물체가 원래 위치에서 사라졌는가? 그리퍼 근처에 있는가?
놓기 검증: 물체가 목표 영역에 나타났는가? 그리퍼에서 사라졌는가?

간단한 시각 검증 코드는 다음과 같은 형태가 될 수 있다.

def verify_object_removed(detector, image_after, target_label, old_bbox):
    objects = detector.detect(image_after)
    for obj in objects:
        if obj.label != target_label:
            continue
        if iou(obj.bbox, old_bbox) > 0.3:
            return False
    return True

여기서 IoU는 두 바운딩 박스가 얼마나 겹치는지 나타내는 값이다. 집기 전에 있던 위치와 집기 후 검출된 위치가 많이 겹친다면 물체가 그대로 남아 있을 가능성이 있다.

물론 이 방식도 완벽하지 않다. 로봇팔이 카메라 시야를 가리면 물체가 사라진 것처럼 보일 수 있다. 조명이 바뀌면 검출이 실패할 수도 있다. 그래서 검증은 하나의 신호만 믿기보다 여러 신호를 함께 보는 것이 좋다. 그리퍼 힘 센서, 관절 토크 변화, 카메라 검출 결과, 깊이 변화가 함께 맞아떨어질 때 성공으로 판단하면 더 안정적이다.

실패를 다루는 재시도 흐름

이번 프로젝트에서 가장 흥미로운 부분은 실패 처리다. 로봇은 거의 반드시 실패한다. 첫날부터 모든 물체를 우아하게 집는 로봇은 없다. 문제는 실패 자체가 아니라 실패 후의 반응이다.

대표적인 실패 상황을 단계별로 정리해 보자.

이 표는 단순한 체크리스트가 아니다. 나중에 LLM 로봇 에이전트를 만들 때 매우 중요한 인터페이스가 된다. LLM에게 "실패했다"라고만 알려 주면 다음 계획을 세우기 어렵다. 하지만 "grasp 단계에서 empty_grasp가 발생했고, 물체는 원래 위치에 남아 있다"라고 알려 주면 훨씬 합리적인 재계획이 가능하다.

재시도 전략은 처음에는 단순하게 만들자.

MAX_RETRY = 3

for attempt in range(MAX_RETRY):
    result = vision_pick_once(target_label="cup", place_name="right_bin")
    if result.success:
        break

    if result.reason == "target_not_found":
        rotate_camera_or_wait()
    elif result.reason == "invalid_depth":
        use_alternative_pixel_region()
    elif result.reason == "plan_failed":
        try_next_grasp_candidate()
    elif result.reason == "empty_grasp":
        adjust_grasp_offset()
    else:
        stop_safely()
        break

여기서 중요한 원칙은 무한 반복하지 않는 것이다. 로봇은 실패할수록 위험해질 수 있다. 물체를 계속 밀어서 책상 가장자리로 보낼 수도 있고, 같은 장애물에 반복해서 접근할 수도 있다. 따라서 재시도 횟수를 제한하고, 실패 로그를 남기며, 일정 횟수 이상 실패하면 사람에게 도움을 요청해야 한다.

안전 구역과 실행 전 점검

카메라 기반 조작 프로젝트에서 안전은 선택 사항이 아니다. 특히 카메라 좌표 변환이 잘못되면 로봇팔이 예상치 못한 방향으로 움직일 수 있다. 객체 검출 모델이 엉뚱한 물체를 목표로 선택할 수도 있다. 깊이값이 튀면 물체가 공중이나 바닥 아래에 있다고 계산될 수도 있다.

그래서 실행 전에 반드시 안전 필터를 넣어야 한다. 로봇에게 전달되는 목표 좌표가 다음 조건을 만족하는지 확인하자.

• 로봇팔의 작업 가능 범위 안에 있는가?

• 책상 표면보다 지나치게 아래에 있지 않은가?

• 사람 또는 금지 구역과 충분히 떨어져 있는가?

• 그리퍼 접근 경로가 카메라, 벽, 고정물과 충돌하지 않는가?

• 좌표 변화가 이전 프레임에 비해 비정상적으로 크지 않은가?

간단한 작업 공간 필터는 다음처럼 만들 수 있다.

def is_inside_workspace(p):
    x, y, z = p
    if x < 0.15 or x > 0.70:
        return False
    if y < -0.40 or y > 0.40:
        return False
    if z < 0.02 or z > 0.50:
        return False
    return True

이 값들은 예시일 뿐이다. 실제 로봇팔의 크기, 책상 위치, 설치 환경에 맞게 조정해야 한다. 처음에는 작업 범위를 좁게 잡는 편이 좋다. 성공률보다 안전이 먼저다. 로봇이 할 수 있는 일을 조금 제한하면 개발자는 훨씬 마음 편히 실험할 수 있다.

또 하나의 좋은 습관은 "실행 전 미리보기"다. MoveIt2가 계획한 경로를 바로 실행하지 말고 RViz에서 먼저 확인한다. 로봇팔이 물체를 향해 자연스럽게 접근하는지, 책상을 뚫고 내려가지는 않는지, 카메라를 치지는 않는지 확인하자. 실제 실행은 계획이 눈으로도 납득될 때 시작한다.

프로젝트 파일 구조 예시

실제 프로젝트를 만들 때는 파일을 적절히 나누는 것이 좋다. 다음은 ROS2 패키지 형태의 간단한 예시다.

vision_pick_project/
  package.xml
  setup.py
  resource/
  launch/
    vision_pick.launch.py
  config/
    camera.yaml
    workspace.yaml
    grasp_params.yaml
  vision_pick_project/
    __init__.py
    camera_buffer.py
    detector.py
    pose_estimator.py
    transform_utils.py
    grasp_planner.py
    moveit_executor.py
    gripper_client.py
    verifier.py
    vision_pick_node.py

각 파일의 역할은 이름 그대로다. detector.py는 객체 인식만 담당한다. pose_estimator.py는 픽셀과 깊이값을 3D 점으로 바꾼다. transform_utils.py는 TF 변환을 다룬다. grasp_planner.py는 집기 후보를 만들고 점수화한다. moveit_executor.py는 MoveIt2를 통해 경로를 계획하고 실행한다. verifier.py는 작업 후 결과를 확인한다.

이렇게 나누면 나중에 모듈을 쉽게 교체할 수 있다. 색상 기반 검출을 쓰다가 YOLO 모델로 바꾸고 싶다면 detector.py만 바꾸면 된다. 바운딩 박스 중심을 쓰다가 세그멘테이션 기반 집기점으로 바꾸고 싶다면 pose_estimator.py와 grasp_planner.py를 개선하면 된다. MoveIt2 설정을 바꾸더라도 인식 코드는 그대로 둘 수 있다.

한 번에 성공시키려 하지 말고 단계별로 켜기

통합 프로젝트에서 가장 흔한 실수는 모든 기능을 한꺼번에 켜는 것이다. 카메라, 검출 모델, TF, MoveIt2, 그리퍼, 검증까지 동시에 실행하면 문제가 생겼을 때 원인을 찾기 어렵다. 로봇이 엉뚱한 곳으로 움직였을 때, 객체 검출이 잘못된 것인지, 깊이값이 틀린 것인지, 좌표 변환이 반대로 된 것인지, 집기 자세가 이상한 것인지 알 수 없다.

다음 순서로 하나씩 검증하자.

1. RGB 이미지와 깊이 이미지가 정상적으로 들어오는지 확인한다.

2. 객체 인식 결과를 이미지 위에 그려서 확인한다.

3. 선택된 픽셀의 깊이값이 현실적인지 확인한다.

4. 카메라 기준 3D 점을 RViz에 표시한다.

5. 로봇 베이스 기준으로 변환한 점을 RViz에 표시한다.

6. 물체 위치 위에 pre_grasp, grasp, post_grasp 마커를 표시한다.

7. MoveIt2 계획만 수행하고 실행은 하지 않는다.

8. 낮은 속도로 실제 로봇팔을 움직인다.

9. 그리퍼를 열고 닫는 동작을 따로 테스트한다.

10. 전체 Pick and Place를 실행하고 카메라 검증을 붙인다.

이 순서를 지키면 프로젝트가 훨씬 안정적으로 진행된다. 특히 4번과 5번은 매우 중요하다. "계산된 점이 RViz에서 실제 물체 위에 보이는가?"라는 질문에 답할 수 있어야 한다. 이 질문에 답하지 못한 상태에서 로봇팔을 움직이는 것은 눈을 감고 다트를 던지는 것과 비슷하다.

디버깅 로그 남기기

통합 시스템은 로그가 생명이다. 실패한 순간의 이미지, 검출 결과, 깊이값, 변환된 좌표, 선택된 집기 후보, MoveIt2 계획 결과를 남겨야 한다. 그래야 같은 실패가 반복될 때 원인을 분석할 수 있다.

최소한 다음 정보는 저장하자.

• 실행 시간과 작업 ID

• 목표 물체 label과 선택된 검출 결과

• RGB 이미지, 깊이 이미지, 검출 시각화 이미지

• 픽셀 좌표, 깊이값, 카메라 기준 3D 좌표

• 로봇 베이스 기준 3D 좌표

• 선택된 grasp pose와 place pose

• MoveIt2 계획 성공 여부와 실패 단계

• 그리퍼 상태와 집기 검증 결과

• 최종 성공 또는 실패 reason

로그는 처음에는 귀찮아 보인다. 하지만 로봇이 실패했을 때 로그가 없으면 기억과 추측에 의존해야 한다. "아마 깊이가 이상했던 것 같은데?" "아마 TF가 틀렸던 것 같은데?" 같은 대화만 반복된다. 반대로 로그가 있으면 실패는 데이터가 된다. 실패 데이터가 쌓이면 다음 장들에서 다룰 모방학습과 강화학습의 출발점이 된다.

첫 성공의 모습

모든 준비가 끝나면 드디어 실행할 차례다. 책상 위에 목표 물체를 놓고 카메라 화면을 확인한다. 인식 결과가 물체 위에 그려진다. 중심 픽셀의 깊이값이 계산된다. RViz에 작은 마커가 나타난다. 마커는 물체 위에 조용히 떠 있다. MoveIt2가 경로를 계획하고, 로봇팔은 천천히 pre_grasp 위치로 이동한다.

그리퍼가 열린다. 팔이 물체 위로 내려온다. 그리퍼가 닫힌다. 잠깐의 정적. 로봇팔이 위로 올라간다. 물체가 함께 따라 올라오면 실험실 분위기가 달라진다. 지금까지 화면 속 숫자와 좌표였던 것들이 현실의 움직임이 되었기 때문이다. 로봇팔은 목표 위치로 이동하고, 물체를 내려놓고, 그리퍼를 연다. 카메라는 다시 장면을 본다. 원래 위치에는 물체가 없고, 목표 영역에는 물체가 있다. 시스템은 success를 반환한다.

이 순간은 단순한 Pick and Place 성공이 아니다. 카메라, 딥러닝 인식, 3D 기하, 좌표 변환, 경로 계획, 그리퍼 제어, 실패 검증이 하나의 행동으로 연결된 순간이다. 로봇 AI가 "생각"에서 "행동"으로 넘어가는 첫 번째 완성된 경험이다.

이 프로젝트가 다음 단계로 이어지는 방식

이번 장에서 만든 시스템은 아직 정해진 물체를 정해진 위치로 옮기는 구조다. 하지만 이미 중요한 뼈대는 갖추었다. 이 뼈대 위에 기능을 하나씩 더하면 훨씬 똑똑한 로봇으로 발전할 수 있다.

자연어 명령을 붙이면 사용자가 "빨간 컵을 오른쪽에 놓아줘"라고 말할 수 있다. 그러면 LLM은 목표 물체와 놓을 위치를 추출하고, 우리가 만든 vision_pick 스킬을 호출한다. 모방학습을 붙이면 사람이 보여 준 집기 동작을 데이터로 모아 더 자연스러운 행동을 학습할 수 있다. 강화학습을 붙이면 시뮬레이션에서 수많은 실패와 성공을 반복하며 더 안정적인 접근 방향이나 힘 조절을 배울 수 있다.

그래서 이번 프로젝트는 3부의 마무리이면서 다음 부의 출발점이다. 우리는 이제 로봇팔을 코드로 움직이는 수준을 넘어, 카메라로 세상을 보고 물체를 다루는 시스템을 만들었다. 다음에는 사람이 직접 규칙을 모두 쓰지 않고, 로봇이 사람의 시연을 보고 행동을 배우는 방법으로 넘어간다. 로봇은 이제 단순히 명령을 실행하는 기계가 아니라, 데이터를 통해 더 나은 행동을 배울 준비가 되었다.