전체 과정 P1 로봇과 실습 도구 이해 · 약 200분 · 중급

CH02URDF 로 보는 로봇 구조와 좌표계

URDF 3D 뷰어로 myAGV Plus 의 링크 · 관절 구조를 살펴보고, base_footprint · base_link · laser · camera_link 같은 좌표계와 TF 변환을 직접 계산합니다.

선수 챕터: CH01
URDF 뷰어Python Playground

학습 목표#

  • URDF 파일에서 링크(link)와 관절(joint)을 구분하고, 부모 · 자식 관계를 따라 로봇의 트리 구조를 그릴 수 있습니다.
  • fixed · continuous · revolute 관절의 차이를 시뮬레이터에서 확인하고, myAGV Plus 의 바퀴 · 센서 관절이 어떤 종류여야 하는지 설명할 수 있습니다.
  • ROS 좌표계 규칙(REP-103)과 map → odom → base_footprint → 센서 프레임 사슬을 설명하고, rpy 값이 센서 축을 어떻게 돌리는지 예측할 수 있습니다.
  • 3×3 동차 변환 행렬로 센서 점을 지도 좌표로 옮기고(합성), 지도 위 목표를 로봇 기준으로 바꿀(역변환) 수 있습니다.
  • URDF 의 <origin xyz rpy> 를 고쳐 센서 위치를 바꾸고, 단위 · 경로 · 각도 실수로 생기는 오류를 찾아낼 수 있습니다.
이 차시를 공부하는 방법

이 페이지에는 브라우저에서 바로 동작하는 미니 시뮬레이터 6개가 있습니다. 시뮬레이터마다 먼저 결과를 예측해 적고, 조작해서 확인하세요. 행렬 계산은 Playground에서 열기 버튼으로 numpy 코드를 바로 실행합니다. 손으로 한 번, 코드로 한 번 계산해 두 값이 같아야 다음으로 넘어갑니다.

1교시 · URDF 의 구조: 링크와 관절#

URDF 는 로봇의 "설계 도면"입니다#

URDF(Unified Robot Description Format)는 로봇의 모양 · 질량 · 관절을 XML 로 적은 파일입니다. 한 번 적어 두면 여러 프로그램이 같은 파일을 읽습니다.

누가 읽나무엇에 쓰나
RViz · 이 강좌의 URDF 뷰어로봇을 3D 로 그림 (<visual>)
물리 시뮬레이터 (Gazebo 등)충돌 · 관성 계산 (<collision> · <inertial>)
robot_state_publisher → TF링크 사이의 좌표 변환을 방송 → 센서 데이터를 한 좌표계로 모음 (2교시)

URDF 는 두 가지 태그로 이루어집니다.

  • <link> — 로봇의 몸체 조각 하나이자 좌표계 하나입니다. 안에 세 가지 모양 정보를 둘 수 있습니다.
    • <visual>: 화면에 보이는 모양 (메시 · 색)
    • <collision>: 충돌 계산용 모양. 계산이 빨라야 하므로 상자 · 원기둥 · 구처럼 단순하게 만듭니다.
    • <inertial>: 질량과 관성 (시뮬레이터 물리 계산용)
  • <joint> — 부모 링크와 자식 링크를 잇는 연결입니다. 자식이 부모 기준 어디에 붙는지(<origin xyz rpy>)와 어떻게 움직일 수 있는지(type)를 적습니다.
관절 type움직임한계 <limit>예
fixed움직이지 않음없음차체에 붙은 센서, 상판
continuous한 축으로 끝없이 회전없음 (속도 · 힘만)? (시뮬레이터에서 찾아봅니다)
revolute한 축으로 회전, 각도 한계 있음lower · upper (rad)로봇팔 관절
prismatic한 축으로 직선 이동lower · upper (m)리프트 · 승강 상판

관절마다 자식은 하나, 링크마다 부모 관절은 하나뿐이므로 URDF 는 나무(트리) 모양이 됩니다. 부모가 없는 링크 하나가 뿌리(root)입니다.

교육용 myAGV Plus 모델의 링크 · 관절 트리 base_footprint base_linklaser_frame camera_linkimu_link wheel_flwheel_fr wheel_rlwheel_rr base_joint (fixed)laser_joint (fixed) camera_joint (fixed)imu_joint (fixed) wheel_*_joint × 4 (type ?) 링크(link): 몸체 조각 · 좌표계 하나 관절(joint): 부모 → 자식 연결 + 상대 위치 바퀴 관절의 type 은 1교시 시뮬레이터에서 직접 확인합니다
이 차시의 교육용 myAGV Plus 모델. 뿌리는 바닥에 붙은 base_footprint 이고, 차체 · 센서가 가지로 뻗습니다.

실제 저장소의 URDF 읽기: myagv_ros2#

myagv_ros2 저장소의 myagv_description/urdf/myAGV.urdf 를 열어 보면 생각보다 짧습니다. 메시 · 색 태그를 줄이면 아래가 전부입니다.

xml
<robot name="myAGV">
  <link name="base_footprint">
    <visual>
      <geometry><mesh filename="package://myagv_description/meshes/myagv_base.dae"/></geometry>
    </visual>
  </link>
  <joint name="base_joint" type="fixed">
    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
    <parent link="base_footprint"/>
    <child link="base_link"/>
  </joint>
  <link name="base_link">
    <visual>
      <geometry><mesh filename="package://myagv_description/meshes/myagv_up.dae"/></geometry>
    </visual>
  </link>
</robot>
  • 링크는 base_footprint(아래 차체 메시) 와 base_link(윗부분 메시) 두 개, 관절은 fixed 하나뿐입니다.
  • 바퀴 링크 · 센서 링크가 없습니다. 바퀴 회전은 화면에 그리지 않고, 센서 위치는 URDF 대신 런치 파일의 정적 TF(2교시)로 알려 줍니다.
  • <collision> · <inertial> 도 없으므로 이 파일만으로는 물리 시뮬레이션을 할 수 없습니다.
myAGV 와 myAGV Plus 는 다른 로봇입니다

myagv_ros2 저장소는 이전 모델 myAGV 를 기준으로 만들어졌습니다. myAGV Plus 의 URDF · 메시 · 센서 장착 위치가 같은지는 확인 필요입니다. 이 차시는 저장소의 실제 값(카메라 · IMU · LiDAR TF)을 그대로 쓰되, 바퀴와 LiDAR 높이처럼 저장소에 없는 값은 교육용 가정으로 채운 모델을 씁니다. 가정한 값은 매번 표시합니다.

시뮬레이터 1 · URDF 탐색기#

교육용 myAGV Plus 모델(링크 9개 · 관절 8개)입니다. 캔버스를 드래그하면 시점이 돌고, 링크 이름 · 원점을 클릭하거나 오른쪽 트리의 버튼을 누르면 그 링크가 선택됩니다. 아래 상자에는 선택한 링크와 부모 관절의 URDF 코드가 나옵니다. 관절 type 은 처음엔 ? 로 가려져 있습니다.

URDF 탐색기 · 교육용 myAGV Plus (치수 일부는 교육용 가정)

해 볼 것 (먼저 예측을 적고 확인하세요)
  1. 트리에서 base_footprint → base_link → wheel_fl 순서로 눌러 봅니다. 원점의 높이(z)가 0 → 0.05 → 0.05 로 바뀌나요? base_footprint 가 바닥에 있는 이유를 생각해 봅니다.
  2. 예측: 바퀴 관절의 type 은 무엇일까요? 적어 둔 뒤 q 슬라이더를 끝까지(720°) 밀고, ▶ 계속 돌리기도 눌러 봅니다. 멈추는 각도가 있나요? 그 다음 관절 type 공개를 켜서 확인합니다.
  3. 바퀴 관절 type 바꿔 보기 를 revolute 로 바꾸고 q 를 180° 로 보내 봅니다. 바퀴가 어디서 멈추나요? fixed 로 바꾸면요? 어느 type 이 바퀴에 맞는지 이유를 말해 봅니다.
  4. collision 보기로 바꿉니다. 바퀴의 충돌 모양은 무엇인가요? 차체의 visual(두 조각) 과 collision(한 덩어리)은 어떻게 다른가요?
  5. imu_link 를 선택합니다. 원점이 base_footprint 와 같은 곳인데, 좌표축 방향은 같나요? (이유는 2교시에서 계산합니다.)
관찰 포인트
  • 링크를 고를 때마다 부모 관절의 origin 이 바뀝니다. 링크의 위치는 언제나 "부모 기준 얼마나 떨어져 있나"로만 적힙니다. 바퀴는 base_link 기준 (±0.085, ±0.135, 0) 이고, base_link 가 바닥에서 0.05 m 위에 있으므로 바퀴 중심 높이는 0.05 m = 바퀴 반지름입니다.
  • 바퀴는 continuous 입니다. 각도 한계가 없어야 계속 굴러갈 수 있습니다. revolute 로 적으면 ±90° 에서 멈추고, fixed 로 적으면 아예 돌지 않습니다. URDF 의 type 이 틀리면 RViz 에서 바퀴가 엉뚱하게 멈춘 것처럼 보입니다.
  • 이 모델에서 바퀴의 collision 은 구(sphere) 입니다. 메카넘 휠의 롤러 모양을 그대로 충돌 계산에 쓰면 느리고 튀기 쉬워서, 시뮬레이션용 모델은 이렇게 단순화하는 경우가 많습니다 (실제 제품 모델의 방식은 확인 필요).
  • imu_link 는 base_footprint 와 원점은 같지만 x · y 축이 반대입니다. 위치가 같아도 방향이 다를 수 있다는 것이 2교시의 출발점입니다.
URDF 뷰어(강좌 도구)로도 확인하기

3D 뷰어 열기

강좌용 myAGV Plus 모델(assets/robot/robot.json)이 준비되면 같은 실습을 3D 뷰어에서 합니다: 링크 이름 표시를 켜고, 트리에서 링크를 클릭해 위치를 확인하고, collision 보기와 visual 보기를 전환합니다. 모델이 아직 없으면 뷰어에 "모델 없음" 안내가 나오므로 위의 페이지 안 탐색기와 실습 과제 1-1 의 Python 코드로 대신합니다.

실습 과제 1#

실습 과제 1-1 · URDF 를 읽어 트리 출력하기

Python 표준 라이브러리 xml.etree.ElementTree 로 URDF 를 읽어 링크 · 관절을 세고 트리를 출력합니다. 교육용 모델과 저장소의 실제 myAGV.urdf 구조를 같은 함수로 출력해 비교하세요. TODO 두 곳을 채웁니다.

python시뮬 실행 가능
import xml.etree.ElementTree as ET

# 교육용 myAGV Plus 모델 (형상 태그는 생략하고 구조만 남김)
EDU_URDF = """<robot name="myagv_plus_edu">
  <link name="base_footprint"/>
  <link name="base_link"/>
  <link name="wheel_fl"/> <link name="wheel_fr"/>
  <link name="wheel_rl"/> <link name="wheel_rr"/>
  <link name="laser_frame"/> <link name="camera_link"/> <link name="imu_link"/>
  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/><child link="base_link"/>
    <origin xyz="0 0 0.05" rpy="0 0 0"/></joint>
  <joint name="wheel_fl_joint" type="continuous">
    <parent link="base_link"/><child link="wheel_fl"/>
    <origin xyz="0.085 0.135 0" rpy="0 0 0"/><axis xyz="0 1 0"/></joint>
  <joint name="wheel_fr_joint" type="continuous">
    <parent link="base_link"/><child link="wheel_fr"/>
    <origin xyz="0.085 -0.135 0" rpy="0 0 0"/><axis xyz="0 1 0"/></joint>
  <joint name="wheel_rl_joint" type="continuous">
    <parent link="base_link"/><child link="wheel_rl"/>
    <origin xyz="-0.085 0.135 0" rpy="0 0 0"/><axis xyz="0 1 0"/></joint>
  <joint name="wheel_rr_joint" type="continuous">
    <parent link="base_link"/><child link="wheel_rr"/>
    <origin xyz="-0.085 -0.135 0" rpy="0 0 0"/><axis xyz="0 1 0"/></joint>
  <joint name="laser_joint" type="fixed">
    <parent link="base_footprint"/><child link="laser_frame"/>
    <origin xyz="0.065 0 0.16" rpy="0 0 3.14159"/></joint>
  <joint name="camera_joint" type="fixed">
    <parent link="base_footprint"/><child link="camera_link"/>
    <origin xyz="0.13 0 0.131" rpy="0 0 0"/></joint>
  <joint name="imu_joint" type="fixed">
    <parent link="base_footprint"/><child link="imu_link"/>
    <origin xyz="0 0 0" rpy="3.14159 3.14159 0"/></joint>
</robot>"""

# myagv_ros2 저장소의 myAGV.urdf 구조 (메시 · 재질 태그 생략)
REPO_URDF = """<robot name="myAGV">
  <link name="base_footprint"/>
  <joint name="base_joint" type="fixed">
    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
    <parent link="base_footprint"/><child link="base_link"/></joint>
  <link name="base_link"/>
</robot>"""

def read_urdf(text):
    root = ET.fromstring(text)
    links = [l.get("name") for l in root.findall("link")]
    joints = []
    for j in root.findall("joint"):
        joints.append((j.get("name"), j.get("type"),
                       j.find("parent").get("link"), j.find("child").get("link")))
    return root.get("name"), links, joints

def show_tree(text):
    name, links, joints = read_urdf(text)
    print("==", name, ": 링크", len(links), "개, 관절", len(joints), "개")
    children = {}
    # TODO 1: joints 를 돌며 children[부모 링크] 목록에 (관절 이름, type, 자식 링크) 를 추가하세요
    #         힌트: children.setdefault(parent, []).append((jn, jt, child))

    # TODO 2: 어떤 관절의 자식으로도 나오지 않는 링크(뿌리)를 roots 에 모으세요
    roots = []
    print("뿌리 링크:", roots)

    def show(link, depth):
        for jn, jt, child in children.get(link, []):
            print("  " * depth + "+- [" + jt + "] " + jn + " -> " + child)
            show(child, depth + 1)

    for r in roots:
        print(r)
        show(r, 1)

    types = {}
    for _, jt, _, _ in joints:
        types[jt] = types.get(jt, 0) + 1
    print("관절 type 개수:", types)
    print()

show_tree(EDU_URDF)
show_tree(REPO_URDF)
풀이 코드 보기
python시뮬 실행 가능
import xml.etree.ElementTree as ET

# 교육용 myAGV Plus 모델 (형상 태그는 생략하고 구조만 남김)
EDU_URDF = """<robot name="myagv_plus_edu">
  <link name="base_footprint"/>
  <link name="base_link"/>
  <link name="wheel_fl"/> <link name="wheel_fr"/>
  <link name="wheel_rl"/> <link name="wheel_rr"/>
  <link name="laser_frame"/> <link name="camera_link"/> <link name="imu_link"/>
  <joint name="base_joint" type="fixed">
    <parent link="base_footprint"/><child link="base_link"/>
    <origin xyz="0 0 0.05" rpy="0 0 0"/></joint>
  <joint name="wheel_fl_joint" type="continuous">
    <parent link="base_link"/><child link="wheel_fl"/>
    <origin xyz="0.085 0.135 0" rpy="0 0 0"/><axis xyz="0 1 0"/></joint>
  <joint name="wheel_fr_joint" type="continuous">
    <parent link="base_link"/><child link="wheel_fr"/>
    <origin xyz="0.085 -0.135 0" rpy="0 0 0"/><axis xyz="0 1 0"/></joint>
  <joint name="wheel_rl_joint" type="continuous">
    <parent link="base_link"/><child link="wheel_rl"/>
    <origin xyz="-0.085 0.135 0" rpy="0 0 0"/><axis xyz="0 1 0"/></joint>
  <joint name="wheel_rr_joint" type="continuous">
    <parent link="base_link"/><child link="wheel_rr"/>
    <origin xyz="-0.085 -0.135 0" rpy="0 0 0"/><axis xyz="0 1 0"/></joint>
  <joint name="laser_joint" type="fixed">
    <parent link="base_footprint"/><child link="laser_frame"/>
    <origin xyz="0.065 0 0.16" rpy="0 0 3.14159"/></joint>
  <joint name="camera_joint" type="fixed">
    <parent link="base_footprint"/><child link="camera_link"/>
    <origin xyz="0.13 0 0.131" rpy="0 0 0"/></joint>
  <joint name="imu_joint" type="fixed">
    <parent link="base_footprint"/><child link="imu_link"/>
    <origin xyz="0 0 0" rpy="3.14159 3.14159 0"/></joint>
</robot>"""

# myagv_ros2 저장소의 myAGV.urdf 구조 (메시 · 재질 태그 생략)
REPO_URDF = """<robot name="myAGV">
  <link name="base_footprint"/>
  <joint name="base_joint" type="fixed">
    <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
    <parent link="base_footprint"/><child link="base_link"/></joint>
  <link name="base_link"/>
</robot>"""

def read_urdf(text):
    root = ET.fromstring(text)
    links = [l.get("name") for l in root.findall("link")]
    joints = []
    for j in root.findall("joint"):
        joints.append((j.get("name"), j.get("type"),
                       j.find("parent").get("link"), j.find("child").get("link")))
    return root.get("name"), links, joints

def show_tree(text):
    name, links, joints = read_urdf(text)
    print("==", name, ": 링크", len(links), "개, 관절", len(joints), "개")
    children = {}
    for jn, jt, parent, child in joints:
        children.setdefault(parent, []).append((jn, jt, child))

    child_set = {child for _, _, _, child in joints}
    roots = [l for l in links if l not in child_set]
    print("뿌리 링크:", roots)

    def show(link, depth):
        for jn, jt, child in children.get(link, []):
            print("  " * depth + "+- [" + jt + "] " + jn + " -> " + child)
            show(child, depth + 1)

    for r in roots:
        print(r)
        show(r, 1)

    types = {}
    for _, jt, _, _ in joints:
        types[jt] = types.get(jt, 0) + 1
    print("관절 type 개수:", types)
    print()

show_tree(EDU_URDF)
show_tree(REPO_URDF)

실행 결과: 교육용 모델은 링크 9개 · 관절 8개(fixed 4, continuous 4), 뿌리는 base_footprint 하나입니다. 저장소 URDF 는 링크 2개 · fixed 관절 1개입니다. 뿌리가 두 개 이상 나오면 트리가 끊긴 것이고, ROS 는 그 URDF 를 거부합니다 (자주 나는 오류 4).

확인 문제 1-1

시뮬레이터 1 에서 바퀴 관절 명령 q 를 720° 로 보내고 계속 돌리기를 눌러도 바퀴가 멈추지 않았습니다. URDF 의 바퀴 관절 type 은?

  1. fixed
  2. revolute
  3. continuous
  4. prismatic
정답 보기③ — 각도 한계 없이 계속 도는 회전 관절은 continuous 입니다. revolute 는 `limit` 의 lower · upper 에서 멈추고(해 볼 것 3), fixed 는 명령이 무시되며, prismatic 은 직선 운동입니다.
확인 문제 1-2

collision 보기에서 바퀴가 롤러 달린 원기둥이 아니라 구(sphere) 로 보였습니다. 가장 알맞은 해석은?

  1. 3D 뷰어의 오류이므로 visual 과 같은 모양으로 고쳐야 한다
  2. 충돌 계산을 빠르고 안정적으로 하려고 일부러 단순한 모양을 쓴 것이다
  3. 바퀴가 실제로 구 모양이라는 뜻이다
  4. collision 은 화면 표시용이라 아무 의미가 없다
정답 보기② — collision 은 시뮬레이터가 충돌 · 접촉을 계산할 때 쓰므로 단순할수록 빠르고 튀지 않습니다. 보이는 모양은 visual 이 따로 맡습니다.
확인 문제 1-3

myagv_ros2 저장소의 myAGV.urdf 에 대한 설명으로 옳은 것은?

  1. 바퀴 4개가 continuous 관절로 들어 있다
  2. LiDAR 와 카메라 링크가 fixed 관절로 들어 있다
  3. base_footprint 와 base_link 두 링크가 fixed 관절 하나로 이어져 있고, 센서 위치는 런치 파일의 정적 TF 가 알려 준다
  4. collision 과 inertial 이 있어 물리 시뮬레이션을 바로 할 수 있다
정답 보기③ — 저장소 URDF 는 메시 두 개를 그리는 최소 구성입니다. camera_link · imu_link · laser_frame 은 런치 파일의 static_transform_publisher 가 방송합니다(2교시).

ROS 의 좌표계 규칙 (REP-103)#

ROS 는 모든 패키지가 같은 약속을 쓰도록 좌표계 규칙(REP-103)을 정해 두었습니다.

  • 오른손 좌표계: 로봇 몸 기준 x = 앞, y = 왼쪽, z = 위
  • 각도는 오른손 법칙: 엄지를 축 방향으로 두었을 때 손가락이 감기는 방향이 +. 위에서 보면 반시계 방향(왼쪽으로 도는 것)이 + 입니다.
  • 단위는 미터(m) 와 라디안(rad). 표시 색은 x · y · z = 빨강 · 초록 · 파랑 (RGB = XYZ)
오른손 좌표계 (REP-103) 위에서 본 로봇 (base_footprint) x (앞) y (왼쪽) z (위) 오른손 엄지를 z 로 → 손가락이 감기는 방향이 + 회전 빨강 x · 초록 y · 파랑 z (RGB = XYZ) 카메라 (앞) x y z (화면 밖으로) θ + (반시계) 각도 단위는 rad (π rad = 180°)
오른손 좌표계와 위에서 본 로봇. θ 가 + 이면 왼쪽으로 돕니다.

표준 프레임 사슬#

자율주행 로봇은 좌표계(프레임)를 여러 개 동시에 씁니다. 각 프레임 사이의 변환을 실시간으로 관리하는 ROS 라이브러리가 TF(tf2) 입니다.

표준 프레임 사슬 (화살표 = 부모 → 자식) mapodombase_footprint base_linklaser_framecamera_linkimu_link 위치추정이 보정(SLAM · AMCL, ch10) 바퀴 · IMU 적분(오도메트리, ch06) URDF launch 의 static TF 센서 프레임: 움직이지 않는 고정 변환 (이 차시) 순간 점프 가능 연속적 · 오차 누적 자식의 부모는 하나뿐 → 사슬을 따라 곱하면 어떤 두 프레임 사이의 변환도 구할 수 있습니다
map → odom → base_footprint → 센서. 이 차시는 오른쪽 절반(고정 변환)을 다루고, 왼쪽 두 칸은 ch06 · ch10 에서 만듭니다.
프레임어디에 붙어 있나누가 알려 주나
map지도의 원점 (움직이지 않음)위치추정 (SLAM · AMCL)
odom로봇이 출발한 곳 근처, 천천히 어긋남오도메트리 (바퀴 · IMU)
base_footprint로봇 중심을 바닥에 내린 점, z = 0(로봇 자신)
base_link차체에 붙은 기준점URDF (robot_state_publisher)
laser_frame · camera_link · imu_link각 센서의 측정 기준점URDF 또는 런치의 정적 TF

myagv_ros2 의 런치 파일 두 곳에서 센서 TF 를 이렇게 방송합니다 (인자 순서: x y z yaw pitch roll 부모 자식).

python
# myagv_odometry/launch/myagv_active.launch.py (발췌)
arguments=['0.13', '0', '0.131', '0', '0', '0', '/base_footprint', '/camera_link']
arguments=['0', '0', '0', '0', '3.14159', '3.14159', '/base_footprint', '/imu_link']

# ydlidar_ros2_driver/launch/ydlidar_launch.py (발췌)
arguments=['0.065', '0.0', '0.08', '3.14159265', '0.0', '0.0', '/base_footprint', '/laser_frame']
인자 순서는 yaw · pitch · roll 입니다

static_transform_publisher 의 숫자 6개는 x y z 다음에 yaw pitch roll 순서입니다. URDF 의 rpy="roll pitch yaw" 와 순서가 반대입니다. 그래서 imu_link 의 0 3.14159 3.14159 는 yaw = 0, pitch = π, roll = π 입니다. URDF 로 옮기면 rpy="3.14159 3.14159 0" 이 됩니다.

  • camera_link: 바닥 중심에서 앞으로 0.13 m, 위로 0.131 m. 회전 없음 → 카메라의 x 축이 로봇 정면을 봅니다.
  • imu_link: 위치는 base_footprint 와 같고 roll · pitch 를 π 만큼 돌렸습니다. 이 센서는 뒤집혀 달린 걸까요? 시뮬레이터 3 에서 직접 확인합니다.
  • laser_frame: yaw = π → LiDAR 의 0° 방향이 로봇 뒤쪽을 봅니다. 저장소 기준 높이는 0.08 m 이지만, 이 차시의 교육용 모델은 LiDAR 를 상판 위(0.16 m)에 둡니다 (myAGV Plus 의 실제 장착 높이는 확인 필요).
camera_link 와 광학 프레임

REP-103 에서 camera_link 처럼 몸체 규칙(x 앞)을 따르는 프레임과, 영상 처리용 광학 프레임(..._optical_frame, z 앞 · x 오른쪽 · y 아래)은 다릅니다. 이 차시의 camera_link 는 몸체 규칙이므로 "카메라 정면 1 m" 는 camera_link 기준 (1, 0, 0) 입니다.

시뮬레이터 2 · 프레임 사슬을 위에서 보기#

로봇을 지도 위에 놓고 프레임들의 축을 한꺼번에 봅니다. 위에서 보므로 z 축은 화면 밖을 향하고, x(빨강) · y(초록)만 보입니다. 카메라 앞 물체 거리 슬라이더는 camera_link 의 x 축 위에 놓은 상자까지의 거리입니다.

프레임 사슬 뷰어 · map → odom → base_footprint → 센서
odom 의 어긋남 (map 기준)
로봇 자세 (odom 기준)
해 볼 것
  1. 예측: 로봇 θ = 0, 물체 거리 1 m 일 때 물체는 base_footprint 기준 어디일까요? 적은 뒤 표의 base_footprint 행과 비교합니다.
  2. 보기: 로봇 주변 확대로 바꿉니다. laser_frame 과 imu_link 의 빨간 x 축은 어느 쪽을 보나요? camera_link 와 비교합니다.
  3. 로봇 θ 를 90° 로 돌립니다. camera_link · base_footprint 행은 그대로인데 odom · map 행만 바뀌는 이유는?
  4. odom 어긋남을 x = 0.3 m, θ = 10° 로 줍니다. 로봇 자세 슬라이더는 그대로인데 지도 위 로봇과 물체가 움직입니다. 표의 마지막 줄 "차이" 가 θ 를 바꿀 때 왜 로봇이 원점에서 멀수록 커지나요?
  5. 물체 거리를 0.3 → 2.0 m 로 늘려 봅니다. laser_frame 기준 x 값의 부호는 어떻게 되나요?
관찰 포인트
  • 카메라 정면 1 m 의 물체는 base_footprint 기준 (1.13, 0, 0.131) 입니다. 센서와 base_footprint 사이에 회전이 없으면 장착 위치만 더하면 됩니다.
  • laser_frame 과 imu_link 는 x 축이 뒤를 봅니다. 그래서 로봇 정면의 물체가 LiDAR 데이터에서는 180° 방향으로 나옵니다. 이것을 모르고 LiDAR 0° 를 "앞" 으로 쓰면 장애물을 정반대에서 찾게 됩니다.
  • 로봇에 붙은 프레임(센서 → base_footprint)은 로봇이 어디에 있든 변하지 않는 고정 변환이고, odom · map 기준 좌표만 로봇 자세에 따라 바뀝니다.
  • map → odom 이 각도 θ 만큼 어긋나면, 원점에서 r 만큼 떨어진 점은 약 r · θ(rad) 만큼 옮겨집니다. 지도 원점에서 멀리 갈수록 작은 각도 오차가 크게 보이는 이유입니다 (ch06 · ch10 에서 다시 다룸).

시뮬레이터 3 · rpy 로 축 돌려 보기#

URDF 의 rpy 는 부모 축 기준으로 x 축으로 roll → y 축으로 pitch → z 축으로 yaw 를 차례로 돌린 회전입니다 (행렬로는 R = Rz(yaw) · Ry(pitch) · Rx(roll)). 점선은 부모(base_footprint) 축, 굵은 선은 회전한 센서 축입니다. 드래그로 시점을 돌립니다.

rpy 축 회전기 · R = Rz(yaw) · Ry(pitch) · Rx(roll)
해 볼 것 (예측 → 확인)
  1. roll 만 180° 를 누르기 전에 x' · y' · z' 가 어디를 볼지 적어 봅니다. 확인 후, 이 상태가 "센서가 뒤집혀 달린 것" 인지 판정합니다.
  2. 예측: imu_link 는 roll 180° 와 pitch 180° 를 둘 다 줍니다. 두 번 뒤집으면 어떻게 될까요? 적은 뒤 imu_link 버튼을 누릅니다.
  3. 2번의 결과를 laser_frame (yaw 180°) 버튼의 결과와 비교합니다. 행렬이 같나요?
  4. roll = 90°, pitch = 90° 를 슬라이더로 만들고 기록합니다. 다음엔 순서만 바꿔 생각해 봅니다: "pitch 를 먼저, 그다음 roll" 로 돌렸다면 같을까요? (힌트: 실습 과제 2-1 에서 행렬 곱 순서를 바꿔 계산)
관찰 포인트
  • roll 180° 만 주면 z' 가 아래를 봅니다 → 이것이 "뒤집혀 달린" 상태입니다.
  • roll 180° + pitch 180° 는 두 번 뒤집어 다시 바로 선 것과 같아서, 결과가 yaw 180° 와 똑같습니다(행렬 diag(−1, −1, 1)). 즉 myagv_ros2 의 imu_link 는 뒤집힌 것이 아니라 수평으로 180° 돌려 달린 것으로 해석됩니다.

"pitch · roll 이 π 니까 센서가 뒤집혔다" 는 직관이 틀릴 수 있다는 것을 계산으로 확인했습니다. 실제 보드의 장착 방향은 실물 확인이 필요합니다.

  • 회전은 순서가 바뀌면 결과가 달라집니다(4번). 그래서 URDF 는 "roll → pitch → yaw (고정축)" 순서를 규칙으로 정해 두었습니다.

실습 과제 2#

실습 과제 2-1 · rpy 를 회전 행렬로

rpy_to_R 을 완성해 세 장착 방향의 축 방향을 출력하고, imu_link 회전이 yaw π 와 같은지 np.allclose 로 확인하세요.

python시뮬 실행 가능
import numpy as np

pi = np.pi

def Rx(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])

def Ry(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])

def Rz(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])

def rpy_to_R(roll, pitch, yaw):
    # TODO: URDF / ROS 규칙 R = Rz(yaw) @ Ry(pitch) @ Rx(roll) 를 돌려주세요
    return np.eye(3)

NAMES = {(1, 0, 0): "앞(+x)", (-1, 0, 0): "뒤(-x)", (0, 1, 0): "왼쪽(+y)",
         (0, -1, 0): "오른쪽(-y)", (0, 0, 1): "위(+z)", (0, 0, -1): "아래(-z)"}

def direction(v):
    key = tuple(int(round(x)) for x in v)
    return NAMES.get(key, str(np.round(v, 2)))

cases = [
    ("laser_frame  rpy=(0, 0, pi)", (0, 0, pi)),
    ("roll 만 pi    rpy=(pi, 0, 0)", (pi, 0, 0)),
    ("imu_link     rpy=(pi, pi, 0)", (pi, pi, 0)),
]
for name, rpy in cases:
    R = rpy_to_R(*rpy)
    print(name)
    print("  센서 x 축 ->", direction(R[:, 0]),
          "/ y 축 ->", direction(R[:, 1]),
          "/ z 축 ->", direction(R[:, 2]))

same = np.allclose(rpy_to_R(pi, pi, 0), rpy_to_R(0, 0, pi))
print("imu_link 회전 == yaw pi 회전 ?", same)
풀이 코드 보기
python시뮬 실행 가능
import numpy as np

pi = np.pi

def Rx(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])

def Ry(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])

def Rz(a):
    c, s = np.cos(a), np.sin(a)
    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])

def rpy_to_R(roll, pitch, yaw):
    # URDF / ROS 규칙: 고정축 기준 x(roll) -> y(pitch) -> z(yaw) 순서로 회전
    return Rz(yaw) @ Ry(pitch) @ Rx(roll)

NAMES = {(1, 0, 0): "앞(+x)", (-1, 0, 0): "뒤(-x)", (0, 1, 0): "왼쪽(+y)",
         (0, -1, 0): "오른쪽(-y)", (0, 0, 1): "위(+z)", (0, 0, -1): "아래(-z)"}

def direction(v):
    key = tuple(int(round(x)) for x in v)
    return NAMES.get(key, str(np.round(v, 2)))

cases = [
    ("laser_frame  rpy=(0, 0, pi)", (0, 0, pi)),
    ("roll 만 pi    rpy=(pi, 0, 0)", (pi, 0, 0)),
    ("imu_link     rpy=(pi, pi, 0)", (pi, pi, 0)),
]
for name, rpy in cases:
    R = rpy_to_R(*rpy)
    print(name)
    print("  센서 x 축 ->", direction(R[:, 0]),
          "/ y 축 ->", direction(R[:, 1]),
          "/ z 축 ->", direction(R[:, 2]))

same = np.allclose(rpy_to_R(pi, pi, 0), rpy_to_R(0, 0, pi))
print("imu_link 회전 == yaw pi 회전 ?", same)

# 덧붙임: 곱하는 순서를 바꾸면? (해 볼 것 4)
a = np.radians(90)
print("Rz Ry Rx 와 Rz Rx Ry 가 같은가?", np.allclose(Rz(0) @ Ry(a) @ Rx(a), Rz(0) @ Rx(a) @ Ry(a)))

실행 결과: laser_frame 과 imu_link 는 모두 x 뒤 · y 오른쪽 · z 위이고, imu_link 회전 == yaw pi 회전 ? True 입니다. roll 만 π 인 경우에만 z 가 아래를 봅니다. 마지막 줄은 False — 회전 순서가 바뀌면 결과가 다릅니다.

실습 과제 2-2 · 센서가 본 점을 base_footprint 로 옮기기

to_base() 의 TODO 를 채워 "센서 기준 점 → base_footprint 기준 점" 을 계산합니다. 공식은 p_base = R · p_sensor + t 입니다 (R 은 장착 회전, t 는 장착 위치).

python시뮬 실행 가능
import numpy as np

pi = np.pi

def rpy_to_R(roll, pitch, yaw):
    cr, sr = np.cos(roll), np.sin(roll)
    cp, sp = np.cos(pitch), np.sin(pitch)
    cy, sy = np.cos(yaw), np.sin(yaw)
    Rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
    Ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]])
    Rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
    return Rz @ Ry @ Rx

# base_footprint -> 센서 (xyz, rpy)
SENSORS = {
    "camera_link": ((0.13, 0.0, 0.131), (0, 0, 0)),     # myagv_ros2 런치 값
    "laser_frame": ((0.065, 0.0, 0.16), (0, 0, pi)),    # x, yaw: myagv_ros2 / z: 교육용 가정
    "imu_link":    ((0.0, 0.0, 0.0), (pi, pi, 0)),      # myagv_ros2 런치 값
}

def to_base(sensor, p):
    """센서 프레임의 점 p 를 base_footprint 기준으로 옮깁니다."""
    xyz, rpy = SENSORS[sensor]
    # TODO: R = rpy_to_R(*rpy), t = np.array(xyz) 로 놓고 R @ p + t 를 돌려주세요
    return np.array(p, dtype=float)

tests = [
    ("camera_link", (1.0, 0.0, 0.0), "카메라 정면 1 m"),
    ("laser_frame", (1.0, 0.0, 0.0), "LiDAR 각도 0 도, 1 m"),
    ("laser_frame", (0.0, 1.0, 0.0), "LiDAR 각도 +90 도, 1 m"),
    ("imu_link",    (0.0, 0.0, 9.8), "IMU 가 읽은 (0, 0, 9.8) 벡터"),
]
for sensor, p, memo in tests:
    q = np.round(to_base(sensor, p), 3) + 0.0
    print("%-11s %-24s -> base_footprint (%.3f, %.3f, %.3f)" % (sensor, memo, q[0], q[1], q[2]))
풀이 코드 보기
python시뮬 실행 가능
import numpy as np

pi = np.pi

def rpy_to_R(roll, pitch, yaw):
    cr, sr = np.cos(roll), np.sin(roll)
    cp, sp = np.cos(pitch), np.sin(pitch)
    cy, sy = np.cos(yaw), np.sin(yaw)
    Rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
    Ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]])
    Rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
    return Rz @ Ry @ Rx

# base_footprint -> 센서 (xyz, rpy)
SENSORS = {
    "camera_link": ((0.13, 0.0, 0.131), (0, 0, 0)),     # myagv_ros2 런치 값
    "laser_frame": ((0.065, 0.0, 0.16), (0, 0, pi)),    # x, yaw: myagv_ros2 / z: 교육용 가정
    "imu_link":    ((0.0, 0.0, 0.0), (pi, pi, 0)),      # myagv_ros2 런치 값
}

def to_base(sensor, p):
    """센서 프레임의 점 p 를 base_footprint 기준으로 옮깁니다."""
    xyz, rpy = SENSORS[sensor]
    R = rpy_to_R(*rpy)
    t = np.array(xyz)
    return R @ np.array(p, dtype=float) + t

tests = [
    ("camera_link", (1.0, 0.0, 0.0), "카메라 정면 1 m"),
    ("laser_frame", (1.0, 0.0, 0.0), "LiDAR 각도 0 도, 1 m"),
    ("laser_frame", (0.0, 1.0, 0.0), "LiDAR 각도 +90 도, 1 m"),
    ("imu_link",    (0.0, 0.0, 9.8), "IMU 가 읽은 (0, 0, 9.8) 벡터"),
]
for sensor, p, memo in tests:
    q = np.round(to_base(sensor, p), 3) + 0.0
    print("%-11s %-24s -> base_footprint (%.3f, %.3f, %.3f)" % (sensor, memo, q[0], q[1], q[2]))

실행 결과: 카메라 정면 1 m → (1.130, 0.000, 0.131). LiDAR 각도 0° 의 1 m 점 → (−0.935, 0.000, 0.160), 즉 로봇 뒤쪽입니다. LiDAR +90° 는 로봇의 오른쪽 (0.065, −1.000, 0.160) 입니다. IMU 의 (0, 0, 9.8) 은 그대로 (0, 0, 9.8) — z 축이 위를 보므로 중력 방향 해석은 바뀌지 않습니다.

확인 문제 2-1

myagv_ros2 런치가 imu_link 에 준 회전(roll = π, pitch = π, yaw = 0)을 시뮬레이터 3 으로 확인했습니다. 옳은 해석은?

  1. IMU 가 위아래로 뒤집혀 달려 있다 (z 축이 아래)
  2. IMU 의 z 축은 위를 보고, x · y 축만 반대 → 수평으로 180° 돌려 단 것과 같다
  3. 두 회전이 서로 지워져 회전이 전혀 없는 것과 같다
  4. IMU 가 옆으로 90° 누워 있다
정답 보기② — Rz(0)·Ry(π)·Rx(π) = diag(−1, −1, 1) 로 yaw π 와 같습니다. ① 은 roll 만 π 일 때입니다. ③ 은 흔한 착각으로, 같은 축의 회전이 아니라서 지워지지 않습니다.
확인 문제 2-2

카메라(camera_link, base_footprint 기준 (0.13, 0, 0.131), 회전 없음)가 정면 1 m 에 물체를 보았습니다. base_footprint 기준 물체 위치는?

  1. (1.00, 0, 0)
  2. (0.87, 0, 0.131)
  3. (1.13, 0, 0.131)
  4. (1.13, 0, 0)
정답 보기③ — 회전이 없으므로 장착 위치 (0.13, 0, 0.131) 을 더합니다. ④ 처럼 높이를 빼먹으면 3D 장애물 높이 판단이 틀어집니다.
확인 문제 2-3

LiDAR 가 yaw = π 로 달려 있습니다(laser_frame). 로봇 정면에 있는 벽은 LiDAR 스캔 데이터에서 몇 도 방향으로 나올까요?

  1. 0°
  2. +90°
  3. −90°
  4. 180° (= −180°)
정답 보기④ — laser_frame 의 x 축이 로봇 뒤를 보므로 정면은 laser_frame 의 180° 입니다(시뮬레이터 2 의 laser_frame 행). TF 를 거쳐 base_footprint 로 옮기면 다시 정면 0° 가 됩니다.

3교시 · 2D 동차 변환으로 센서 데이터 옮기기#

세 숫자 (x, y, θ) 와 3×3 행렬#

바닥을 달리는 로봇의 자세는 x, y, θ 세 숫자로 충분합니다. 이 세 숫자를 3×3 동차 변환 행렬로 쓰면, 회전과 이동을 곱셈 한 번으로 처리할 수 있습니다.

2D 동차 변환 행렬 T(x, y, θ) cosθ−sinθx sinθcosθy 001 회전 R(θ) 이동 t 점 (px, py, 1) 에 곱하면: 먼저 회전, 그다음 이동 합성: 오른쪽부터 읽습니다 p_map = T_map_odom · T_odom_base · T_base_laser · p_laser mapodombaselaser 점은 laser → base → odom → map 순서로 옮겨집니다 아래 첨자가 맞닿게 (…_base · T_base_…) 이어 붙입니다
왼쪽 위 2×2 는 회전, 오른쪽 열은 이동입니다. 점은 (px, py, 1) 처럼 끝에 1 을 붙여 곱합니다.

손으로 한 번 계산해 봅니다. 로봇이 지도의 (2, 1) 에서 90°(왼쪽, 지도 +y 방향)를 보고 있을 때, 로봇 기준 (1, 0) — 로봇 정면 1 m — 은 지도 어디일까요?

  1. 회전: (1, 0) 을 90° 돌리면 (cos90°·1 − sin90°·0, sin90°·1 + cos90°·0) = (0, 1)
  2. 이동: (0, 1) + (2, 1) = (2, 2)

변환 여러 개를 이어 붙일 때는 아래 첨자가 맞닿게 곱합니다.

  • T_map_laser = T_map_odom · T_odom_base · T_base_laser
  • 점은 오른쪽부터 차례로 옮겨집니다: laser → base → odom → map
  • 행렬 곱은 순서를 바꾸면 결과가 달라집니다 (A·B ≠ B·A). 회전 다음 이동과, 이동 다음 회전은 다른 동작이기 때문입니다.

역변환은 "지도 → 로봇" 방향입니다. 회전 행렬의 역은 전치(Rᵀ)이므로 계산이 쉽습니다.

  • T⁻¹ = [[Rᵀ, −Rᵀ·t], [0, 0, 1]]
  • 쓰임: 지도 위 목표점을 로봇 기준으로 바꾸면, 목표가 앞 · 뒤 · 왼쪽 · 오른쪽 중 어디인지 바로 알 수 있습니다 (경로 추종 제어의 첫 단계, ch08).

시뮬레이터 4 · TF 계산기#

로봇 자세와 LiDAR 장착 방식을 바꾸며 LiDAR 의 한 점(파란 점)이 지도 어디에 찍히는지 봅니다. 지도를 클릭하면 목표점(주황) 이 놓이고, 그 목표를 로봇 기준 좌표로 바꾼 결과가 나옵니다.

TF 계산기 · p_map = T_map_odom · T_odom_base · T_base_laser · p_laser
로봇 (odom 기준)
LiDAR 장착스캔 점
해 볼 것 (손으로 먼저, 그다음 확인)
  1. 손 계산 예제로 맞추기를 누릅니다(로봇 (2, 1, 90°), LiDAR 를 로봇 중심에 0° 로 장착, 점 (1, 0)). 위에서 손으로 구한 (2, 2) 와 파란 점의 지도 좌표가 같은가요?
  2. 같은 상태에서 곱셈 순서를 거꾸로를 켭니다. 빨간 × 가 파란 점과 겹치나요? 겹친다면 이유를 생각해 봅니다. 이어서 LiDAR 장착을 x = 0.065 m · yaw 180° 로, α 를 180° 로 바꾸면 빨간 × 는 어디로 가나요?
  3. LiDAR yaw 를 180° (myagv_ros2) 로 바꾸고, 스캔 각도 α 를 0° → 180° 로 바꿔 봅니다. 로봇 정면의 점을 보려면 α 가 몇 도여야 하나요?
  4. map→odom 을 (0.2, −0.1, 5°) 로 바꿉니다. 로봇 슬라이더는 그대로인데 로봇 · 파란 점이 함께 움직입니다. T_map_base 행렬의 어느 칸이 바뀌었나요?
  5. 로봇을 (2, 1, 90°) 로 두고 지도의 (3, 3) 을 클릭합니다. 목표는 로봇의 어느 쪽인가요? 이어서 (1, 1) 과 (2, 0) 도 클릭해 봅니다.
관찰 포인트
  • 1번의 정답은 (2, 2) 입니다. 손 계산 예제에서는 T_map_odom 과 T_base_laser 가 "아무 변환 없음"(단위행렬)이라 순서를 바꿔도 결과가 같습니다. 그래서 간단한 예제 하나로는 순서 실수가 드러나지 않습니다.
  • LiDAR 장착 변환(x 0.065 · yaw 180°)이 생기는 순간, 거꾸로 곱한 점은 (−1.935, 0) 으로 지도 밖으로 튑니다(바른 값은 (2, 2.065)). 점 하나 안에서도 마찬가지로, 회전보다 이동을 먼저 하면 (2, 1) 을 먼저 더한 (3, 1) 을 90° 돌려 (−1, 3) 이 됩니다. 점이 엉뚱한 곳에 찍히면 곱셈 순서부터 의심합니다.
  • yaw 180° 로 단 LiDAR 는 α = 180° 가 로봇 정면입니다. 같은 스캔 데이터라도 T_base_laser 를 정확히 알아야 지도에 제자리에 찍힙니다.
  • map→odom 이 바뀌면 T_map_base 의 회전 칸(cos · sin)과 이동 칸이 함께 바뀝니다. 로봇 자신은 아무것도 모른 채 지도 위 위치만 고쳐지는 것 — 이것이 위치추정이 하는 일입니다(ch10).
  • 로봇 (2, 1, 90°) 에서 목표 (3, 3) 은 로봇 기준 (2, −1): 앞으로 2 m, 오른쪽으로 1 m → 오른쪽 앞입니다. (1, 1) 은 (0, 1) 로 바로 왼쪽, (2, 0) 은 (−1, 0) 으로 바로 뒤입니다.

실습 과제 3#

실습 과제 3-1 · numpy TF 계산기

T2() 는 완성되어 있습니다. 사슬 합성 T_map_laser 와, 순서를 거꾸로 곱한 T_wrong 을 채워 세 스캔 점을 지도로 옮기세요.

python시뮬 실행 가능
import numpy as np

def T2(x, y, theta_deg):
    """2D 동차 변환 행렬 (각도는 도로 받아 rad 로 바꿈)"""
    th = np.radians(theta_deg)
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s, x],
                     [s,  c, y],
                     [0,  0, 1.0]])

def apply(T, p):
    q = T @ np.array([p[0], p[1], 1.0])
    return q[:2]

# 1) 손 계산 확인: 로봇이 (2, 1, 90 도), 로봇 기준 (1, 0) 의 점
T_map_base = T2(2, 1, 90)
print("로봇 기준 (1, 0) -> 지도", np.round(apply(T_map_base, (1, 0)), 3))

# 2) 사슬 합성: map -> odom -> base_footprint -> laser_frame
T_map_odom = T2(0.2, -0.1, 5)       # 위치추정이 보정한 오차
T_odom_base = T2(2.0, 1.0, 90)      # 오도메트리가 말하는 로봇 자세
T_base_laser = T2(0.065, 0.0, 180)  # LiDAR 장착 (yaw pi)

T_map_laser = np.eye(3)   # TODO: 세 행렬을 아래 첨자가 맞닿게 곱하세요
T_wrong = np.eye(3)       # TODO: 같은 세 행렬을 거꾸로 곱하세요 (오답 확인용)

scan = [(1.0, 0), (1.0, 90), (2.0, 180)]   # (거리 m, LiDAR 각도 도)
for r, a in scan:
    p_laser = (r * np.cos(np.radians(a)), r * np.sin(np.radians(a)))
    ok = apply(T_map_laser, p_laser)
    bad = apply(T_wrong, p_laser)
    print("스캔 r=%.1f a=%4d -> 지도 (%.3f, %.3f) | 순서 반대 (%.3f, %.3f)"
          % (r, a, ok[0], ok[1], bad[0], bad[1]))
풀이 코드 보기
python시뮬 실행 가능
import numpy as np

def T2(x, y, theta_deg):
    """2D 동차 변환 행렬 (각도는 도로 받아 rad 로 바꿈)"""
    th = np.radians(theta_deg)
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s, x],
                     [s,  c, y],
                     [0,  0, 1.0]])

def apply(T, p):
    q = T @ np.array([p[0], p[1], 1.0])
    return q[:2]

# 1) 손 계산 확인: 로봇이 (2, 1, 90 도), 로봇 기준 (1, 0) 의 점
T_map_base = T2(2, 1, 90)
print("로봇 기준 (1, 0) -> 지도", np.round(apply(T_map_base, (1, 0)), 3))

# 2) 사슬 합성: map -> odom -> base_footprint -> laser_frame
T_map_odom = T2(0.2, -0.1, 5)       # 위치추정이 보정한 오차
T_odom_base = T2(2.0, 1.0, 90)      # 오도메트리가 말하는 로봇 자세
T_base_laser = T2(0.065, 0.0, 180)  # LiDAR 장착 (yaw pi)

T_map_laser = T_map_odom @ T_odom_base @ T_base_laser
T_wrong = T_base_laser @ T_odom_base @ T_map_odom   # 순서를 거꾸로 곱한 오답

scan = [(1.0, 0), (1.0, 90), (2.0, 180)]   # (거리 m, LiDAR 각도 도)
for r, a in scan:
    p_laser = (r * np.cos(np.radians(a)), r * np.sin(np.radians(a)))
    ok = apply(T_map_laser, p_laser)
    bad = apply(T_wrong, p_laser)
    print("스캔 r=%.1f a=%4d -> 지도 (%.3f, %.3f) | 순서 반대 (%.3f, %.3f)"
          % (r, a, ok[0], ok[1], bad[0], bad[1]))

실행 결과: 손 계산 점은 [2. 2.] 입니다. 사슬 합성의 첫 점(LiDAR 0° = 로봇 뒤 1 m)은 지도 (2.187, 0.139), 순서를 거꾸로 하면 (−1.948, −2.196) 처럼 지도 밖의 엉뚱한 곳이 나옵니다. 시뮬레이터 4 에서 같은 값(로봇 (2, 1, 90°), map→odom (0.2, −0.1, 5°), LiDAR x 0.065 · yaw 180°, r = 1, α = 0°)을 넣어 결과를 맞춰 보세요.

실습 과제 3-2 · 역변환으로 목표 방향 판단하기

inverse() 를 공식 T⁻¹ = [[Rᵀ, −Rᵀ·t], [0, 0, 1]] 로 완성하고 np.linalg.inv 와 같은지 확인합니다. 그다음 네 목표가 로봇의 어느 쪽인지 출력합니다.

python시뮬 실행 가능
import numpy as np

def T2(x, y, theta_deg):
    th = np.radians(theta_deg)
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s, x], [s, c, y], [0, 0, 1.0]])

def inverse(T):
    """동차 변환의 역: [[R, t], [0, 1]]^-1 = [[R^T, -R^T t], [0, 1]]"""
    R = T[:2, :2]
    t = T[:2, 2]
    Ti = np.eye(3)
    # TODO: Ti 의 왼쪽 위 2x2 에 R.T, 오른쪽 열 위 두 칸에 -R.T @ t 를 넣으세요
    return Ti

def describe(p):
    x, y = p
    fb = "앞" if x > 0.05 else ("뒤" if x < -0.05 else "")
    lr = "왼쪽" if y > 0.05 else ("오른쪽" if y < -0.05 else "")
    word = (lr + " " + fb).strip() or "제자리"
    return word

T_map_base = T2(2, 1, 90)
print("직접 만든 역행렬 == np.linalg.inv ?", np.allclose(inverse(T_map_base), np.linalg.inv(T_map_base)))

T_base_map = inverse(T_map_base)
goals = [(3, 3), (1, 1), (2, 0), (2, 2.5)]
for g in goals:
    q = T_base_map @ np.array([g[0], g[1], 1.0])
    ang = np.degrees(np.arctan2(q[1], q[0]))
    print("목표 지도", g, "-> 로봇 기준 (%.2f, %.2f)  방향 %6.1f 도  %s" % (q[0], q[1], ang, describe(q[:2])))
풀이 코드 보기
python시뮬 실행 가능
import numpy as np

def T2(x, y, theta_deg):
    th = np.radians(theta_deg)
    c, s = np.cos(th), np.sin(th)
    return np.array([[c, -s, x], [s, c, y], [0, 0, 1.0]])

def inverse(T):
    """동차 변환의 역: [[R, t], [0, 1]]^-1 = [[R^T, -R^T t], [0, 1]]"""
    R = T[:2, :2]
    t = T[:2, 2]
    Ti = np.eye(3)
    Ti[:2, :2] = R.T
    Ti[:2, 2] = -R.T @ t
    return Ti

def describe(p):
    x, y = p
    fb = "앞" if x > 0.05 else ("뒤" if x < -0.05 else "")
    lr = "왼쪽" if y > 0.05 else ("오른쪽" if y < -0.05 else "")
    word = (lr + " " + fb).strip() or "제자리"
    return word

T_map_base = T2(2, 1, 90)
print("직접 만든 역행렬 == np.linalg.inv ?", np.allclose(inverse(T_map_base), np.linalg.inv(T_map_base)))

T_base_map = inverse(T_map_base)
goals = [(3, 3), (1, 1), (2, 0), (2, 2.5)]
for g in goals:
    q = T_base_map @ np.array([g[0], g[1], 1.0])
    ang = np.degrees(np.arctan2(q[1], q[0]))
    print("목표 지도", g, "-> 로봇 기준 (%.2f, %.2f)  방향 %6.1f 도  %s" % (q[0], q[1], ang, describe(q[:2])))

실행 결과: (3, 3) → (2.00, −1.00) 오른쪽 앞, (1, 1) → (0.00, 1.00) 왼쪽, (2, 0) → (−1.00, 0.00) 뒤, (2, 2.5) → (1.50, 0.00) 앞. 시뮬레이터 4 의 해 볼 것 5 와 같은 값입니다.

확인 문제 3-1

로봇이 지도 (2, 1) 에서 θ = 90° 를 보고 있습니다. 로봇 기준 (1, 0) 의 점은 지도 어디일까요?

  1. (3, 1)
  2. (2, 2)
  3. (−1, 3)
  4. (2, 0)
정답 보기② — (1, 0) 을 90° 돌리면 (0, 1), 여기에 (2, 1) 을 더해 (2, 2). ① 은 회전을 빼먹은 값, ③ 은 이동을 먼저 하고 회전한 값(곱셈 순서 반대), ④ 는 회전 방향을 시계 방향(−90°)으로 잘못 돌린 값입니다.
확인 문제 3-2

LiDAR 점을 지도로 옮기는 식 중 잘못된 것은? (p 는 laser_frame 기준 점)

  1. T_map_odom · T_odom_base · T_base_laser · p
  2. T_map_base · T_base_laser · p (T_map_base = T_map_odom · T_odom_base)
  3. T_map_laser · p (T_map_laser = T_map_odom · T_odom_base · T_base_laser)
  4. T_base_laser · T_odom_base · T_map_odom · p
정답 보기④ — 아래 첨자가 맞닿지 않습니다(laser · odom, base · map). 시뮬레이터 4 의 빨간 × 처럼 엉뚱한 곳에 찍힙니다. ①②③ 은 묶는 방법만 다를 뿐 같은 결과입니다(행렬 곱은 결합법칙이 성립).
확인 문제 3-3

로봇 (2, 1, 90°) 에서 지도의 목표 (3, 3) 을 로봇 기준으로 바꾸었더니 (2, −1) 이 나왔습니다. 목표는 로봇의 어느 쪽인가요?

  1. 왼쪽 앞
  2. 오른쪽 앞
  3. 왼쪽 뒤
  4. 오른쪽 뒤
정답 보기② — x = 2 > 0 이므로 앞, y = −1 < 0 이므로 오른쪽입니다(REP-103: y 는 왼쪽이 +). 지도에서는 로봇의 "위쪽 오른편" 처럼 보여도, 로봇 기준으로 바꿔야 회전 방향을 바로 정할 수 있습니다.

4교시 · 도전: 센서 위치를 바꾼 나만의 URDF#

센서 높이와 시야: 2D LiDAR 는 "얇은 판 하나" 만 봅니다#

myAGV Plus 의 LiDAR 는 한 높이에서 360° 를 도는 2D LiDAR 입니다. 그래서 LiDAR 가 보는 것은 LiDAR 높이의 수평면 하나와 부딪히는 물체뿐입니다.

  • LiDAR 가 높으면: 멀리까지 가리는 것 없이 보지만, 그보다 낮은 물체(팔레트 · 상자)를 놓칩니다.
  • LiDAR 가 낮으면: 낮은 물체는 보지만, 차체 · 바퀴에 가리거나 바닥의 작은 턱을 장애물로 착각하기 쉽습니다.
  • 물체의 위쪽만 튀어나온 경우(작업대 상판, 선반 칸)는 LiDAR 높이에 다리만 걸리면 "기둥 몇 개" 로만 보입니다.

시뮬레이터 5 · LiDAR 높이와 선반 통로#

선반 통로 맵의 한 구간입니다. 위는 옆에서 본 모습, 아래는 위에서 본 LiDAR 스캔입니다. 통로 위에 높이가 다른 물체 4개가 있고, 양쪽은 선반입니다(선반 맨 아래 칸 0.06~0.12 m, 기둥 0.6 m 간격).

LiDAR 높이 실험 · 선반 통로 (스캔 평면 = LiDAR 높이)
해 볼 것 (예측 → 확인)
  1. 예측: h = 0.16 m (교육용 모델) 에서 네 물체 중 몇 개가 스캔에 잡힐까요? 확인 후 위 그림(옆모습)에서 스캔 평면이 어느 물체를 지나는지 봅니다.
  2. h 를 0.10 m 부터 0.01 m 씩 올리며 낮은 상자(0.25 m) 가 잡히는 최대 높이를 찾습니다. 팔레트(0.12 m)가 사라지는 높이도 적습니다.
  3. h = 0.10 과 h = 0.13 에서 양옆 선반이 어떻게 다르게 보이나요? 기둥 사이로 빠져나간 광선은 지도에서 무엇으로 기록될까요?
  4. 로봇팔 얹기를 켭니다. h = 0.16 m 에서 작업대 상판의 판정이 어떻게 바뀌나요? 로봇팔이 없을 때와 비교합니다.
  5. 네 물체를 모두 보는 LiDAR 높이가 있나요? 없다면 실제 물류 로봇은 어떻게 보완할까요?
관찰 포인트
  • 스캔에 잡히는 조건은 물체의 아래 높이 ≤ h < 물체의 위 높이 입니다. 낮은 상자는 h = 0.24 m 까지(0.25 m 미만) 잡히고, 팔레트는 h ≥ 0.12 m 부터 사라집니다.
  • h ≥ 0.12 m 에서는 선반이 기둥 점만 남습니다. 기둥 사이로 빠져나간 광선은 "멀리까지 비어 있음" 으로 기록되어, 지도에 선반이 벽이 아닌 점선으로 그려집니다 (ch07 점유 격자에서 다시 봅니다).
  • 로봇팔을 얹으면 로봇의 높이가 0.55 m 가 되어, 스캔 평면보다 높은 작업대 상판(0.45~0.50 m)에 부딪힐 수 있습니다. LiDAR 높이는 그대로인데 위험이 생긴 것 — 센서 배치와 로봇 크기는 함께 따져야 합니다.
  • 한 높이로 모든 물체를 볼 수는 없습니다. 실제 시스템은 카메라 · 깊이 센서 · 범퍼를 더하거나, 금지 구역(keep-out)을 지도에 그려 보완합니다 (ch08 · ch10).

로봇팔을 상판에 얹으면: 트리와 무게중심#

상판에 로봇팔(예: myCobot 270 M5)을 얹을 때는 팔의 베이스 링크를 고정(fixed) 관절로 base_link 에 붙입니다. 두 URDF 가 한 트리로 합쳐집니다.

xml
<!-- 로봇팔 URDF 의 뿌리 링크(예: arm_base)를 차체 상판에 고정 -->
<joint name="arm_mount_joint" type="fixed">
  <parent link="base_link"/>
  <child link="arm_base"/>
  <origin xyz="0.06 0 0.08" rpy="0 0 0"/>
</joint>
  • 이 관절이 없으면 트리의 뿌리가 둘(base_footprint, arm_base)이 되어 ROS 가 URDF 를 거부합니다 (자주 나는 오류 4).
  • 질량이 위쪽 · 앞쪽에 더해지므로 무게중심이 올라가고 앞으로 이동합니다. 팔을 앞으로 뻗어 물건을 들면 더 앞으로 갑니다. 무게중심이 높을수록 제동 · 가속 때 기울어지기 쉽습니다 (ch01 3교시의 적재 기준과 연결).
  • 위 origin 값과 로봇팔의 링크 이름 · 질량은 예시입니다. 실제 장착 위치와 무게는 제품 문서로 확인합니다 (확인 필요).

시뮬레이터 6 · URDF 편집기와 오류 검사#

LiDAR 의 <joint> 를 직접 고쳐 위치를 옮깁니다. 적용 · 검사를 누르면 XML 을 읽어 옆모습 · 윗모습에 LiDAR 를 그리고, 흔한 실수를 찾아 알려 줍니다. 아래 버튼은 일부러 틀린 예제를 불러옵니다. 무엇이 틀렸는지 검사 전에 먼저 찾아 보세요.

URDF 편집기 · laser_joint 고치기
해 볼 것
  1. 정상을 불러와 LiDAR 의 x 축(빨간 선) 방향을 확인합니다. rpy 의 마지막 값을 0 으로 바꾸고 적용 · 검사를 누르면 어떻게 되나요?
  2. LiDAR 를 차체 앞쪽 · 더 높은 곳으로 옮겨 봅니다: xyz 를 0.11 0 0.30 으로 고칩니다 (앞 · 높게 옮기기 버튼과 같음). 어떤 주의가 뜨나요? 시뮬레이터 5 의 결과와 연결해 설명합니다.
  3. 예제 A ~ E 를 하나씩 불러와, 검사 버튼을 누르기 전에 틀린 곳을 찾아 적습니다. 그다음 검사 결과와 비교합니다.
  4. 예제 B 에서 검사기가 알려 준 "실제로 해석되는 각도" 를 확인하고, 올바른 값으로 고쳐 문제 없음이 나오게 만듭니다.
  5. parent 를 base_link 로 바꾸고 xyz 는 그대로 둡니다. LiDAR 높이가 얼마나 바뀌나요? 같은 높이(0.16 m)를 유지하려면 z 를 얼마로 적어야 할까요?
관찰 포인트
  • 예제 A 는 mm 단위(65, 160), B 는 도 단위 rpy(180 → 180 rad 는 약 233° 로 해석), C 는 package:// 경로, D 는 부모 링크 이름 오타, E 는 닫는 태그 누락입니다. 이 중 A · B 는 XML 로는 "정상" 이라 ROS 가 오류를 내지 않고 엉뚱한 위치에 그대로 띄웁니다. 가장 찾기 어려운 실수입니다.
  • 부모를 base_link 로 바꾸면 base_link 가 바닥에서 0.05 m 위이므로 높이가 0.21 m 가 됩니다. 같은 높이를 원하면 z = 0.11 로 적습니다. origin 은 언제나 부모 기준이라는 1교시의 원칙 그대로입니다.
  • LiDAR 를 0.30 m 로 올리면 차체에 가리는 것은 없어지지만, 시뮬레이터 5 에서 본 것처럼 낮은 상자 · 팔레트를 놓칩니다. 센서 위치는 "가림" 과 "놓침" 사이의 절충입니다.

실습 과제 4#

실습 과제 4-1 · URDF 오류 검사기 만들기

시뮬레이터 6 의 검사 규칙 중 두 가지(mm 단위, 도 단위)를 직접 구현합니다. 다섯 개의 laser_joint 조각을 검사해 각 오류를 찾아내세요.

python시뮬 실행 가능
import math
import xml.etree.ElementTree as ET

KNOWN_LINKS = {"base_footprint", "base_link", "laser_frame", "camera_link", "imu_link"}

def lint_joint(text):
    """센서 관절 조각을 검사해 문제 목록을 돌려줍니다."""
    problems = []
    try:
        root = ET.fromstring(text)
    except ET.ParseError as e:
        return ["XML 문법 오류: " + str(e)]
    for mesh in root.iter("mesh"):
        fn = mesh.get("filename", "")
        if fn.startswith("package://"):
            problems.append("package:// 경로 - 웹 뷰어 · 다른 PC 에서 못 찾음: " + fn)
    for j in root.iter("joint"):
        parent = j.find("parent").get("link")
        if parent not in KNOWN_LINKS:
            problems.append("부모 링크 [%s] 없음 (오타?)" % parent)
        origin = j.find("origin")
        xyz = [float(v) for v in origin.get("xyz", "0 0 0").split()]
        rpy = [float(v) for v in origin.get("rpy", "0 0 0").split()]
        # TODO 1: xyz 중 절댓값이 2 m 를 넘는 값이 있으면 "mm 로 적었나요?" 문제를 추가하세요
        # TODO 2: rpy 중 절댓값이 2*pi 를 넘는 값이 있으면 "도(degree)로 적었나요?" 문제를 추가하세요
    return problems

def make(xyz="0.065 0 0.16", rpy="0 0 3.14159", parent="base_footprint", mesh="meshes/lidar.glb"):
    return ('<robot name="t"><link name="laser_frame"><visual><geometry>'
            '<mesh filename="%s"/></geometry></visual></link>'
            '<joint name="laser_joint" type="fixed"><parent link="%s"/>'
            '<child link="laser_frame"/><origin xyz="%s" rpy="%s"/></joint></robot>'
            % (mesh, parent, xyz, rpy))

cases = {
    "A 정상": make(),
    "B": make(xyz="65 0 160"),
    "C": make(rpy="0 0 180"),
    "D": make(mesh="package://myagv_description/meshes/lidar.dae"),
    "E": make(parent="base_foot_print"),
}
for name, text in cases.items():
    p = lint_joint(text)
    print(name, "->", "문제 없음" if not p else "")
    for msg in p:
        print("    -", msg)
풀이 코드 보기
python시뮬 실행 가능
import math
import xml.etree.ElementTree as ET

KNOWN_LINKS = {"base_footprint", "base_link", "laser_frame", "camera_link", "imu_link"}

def lint_joint(text):
    """센서 관절 조각을 검사해 문제 목록을 돌려줍니다."""
    problems = []
    try:
        root = ET.fromstring(text)
    except ET.ParseError as e:
        return ["XML 문법 오류: " + str(e)]
    for mesh in root.iter("mesh"):
        fn = mesh.get("filename", "")
        if fn.startswith("package://"):
            problems.append("package:// 경로 - 웹 뷰어 · 다른 PC 에서 못 찾음: " + fn)
    for j in root.iter("joint"):
        parent = j.find("parent").get("link")
        if parent not in KNOWN_LINKS:
            problems.append("부모 링크 [%s] 없음 (오타?)" % parent)
        origin = j.find("origin")
        xyz = [float(v) for v in origin.get("xyz", "0 0 0").split()]
        rpy = [float(v) for v in origin.get("rpy", "0 0 0").split()]
        if any(abs(v) > 2.0 for v in xyz):
            problems.append("xyz %s - 2 m 넘는 값: mm 로 적었나요? (URDF 는 m)" % xyz)
        if any(abs(v) > 2 * math.pi + 0.01 for v in rpy):
            problems.append("rpy %s - 2 pi 넘는 값: 도(degree)로 적었나요? (URDF 는 rad)" % rpy)
    return problems

def make(xyz="0.065 0 0.16", rpy="0 0 3.14159", parent="base_footprint", mesh="meshes/lidar.glb"):
    return ('<robot name="t"><link name="laser_frame"><visual><geometry>'
            '<mesh filename="%s"/></geometry></visual></link>'
            '<joint name="laser_joint" type="fixed"><parent link="%s"/>'
            '<child link="laser_frame"/><origin xyz="%s" rpy="%s"/></joint></robot>'
            % (mesh, parent, xyz, rpy))

cases = {
    "A 정상": make(),
    "B": make(xyz="65 0 160"),
    "C": make(rpy="0 0 180"),
    "D": make(mesh="package://myagv_description/meshes/lidar.dae"),
    "E": make(parent="base_foot_print"),
}
for name, text in cases.items():
    p = lint_joint(text)
    print(name, "->", "문제 없음" if not p else "")
    for msg in p:
        print("    -", msg)

실행 결과: A 는 문제 없음, B 는 mm 단위, C 는 도 단위, D 는 package:// 경로, E 는 부모 링크 오타가 잡힙니다. 주의: 이 규칙은 "흔한 실수" 를 걸러 낼 뿐입니다. 예를 들어 rpy="0 0 3" 처럼 범위 안이지만 틀린 값(180° 를 의도했는데 3 rad ≈ 172°)은 잡지 못합니다. 마지막 확인은 뷰어에서 눈으로 합니다.

실습 과제 4-2 · LiDAR 높이 찾기와 무게중심 계산

(1) detected() 를 완성해 높이별로 보이는 물체를 출력하고, 낮은 상자가 잡히는 최대 높이를 찾습니다. (2) 로봇팔과 물건을 얹었을 때의 무게중심을 계산합니다. 질량 · 위치는 교육용 가정입니다.

python시뮬 실행 가능
import numpy as np

# 선반 통로의 물체: (이름, 아래 높이 m, 위 높이 m)
OBJECTS = [
    ("팔레트", 0.00, 0.12),
    ("낮은 상자", 0.00, 0.25),
    ("높은 상자", 0.00, 0.40),
    ("작업대 상판", 0.45, 0.50),
]

def detected(h, z_lo, z_hi):
    """2D LiDAR 는 높이 h 의 수평면 하나만 봅니다."""
    # TODO: 스캔 평면 h 가 물체의 [아래 높이, 위 높이) 안에 있으면 True
    return False

# 1) 높이별로 무엇이 보이나
for h in (0.10, 0.20, 0.30, 0.47):
    seen = [n for n, lo, hi in OBJECTS if detected(h, lo, hi)]
    print("LiDAR %.2f m : %s" % (h, ", ".join(seen) if seen else "아무것도 안 보임"))

# 2) 낮은 상자가 잡히는 최대 높이 (0.01 m 간격으로 찾기)
heights = np.round(np.arange(0.10, 0.501, 0.01), 2)
ok = [h for h in heights if detected(h, 0.00, 0.25)]
print("낮은 상자가 잡히는 최대 LiDAR 높이:", max(ok) if ok else "없음", "m")

# 3) 로봇팔을 얹었을 때 무게중심 (질량 · 위치는 교육용 가정 - 확인 필요)
parts = [  # (이름, 질량 kg, x m, z m)  x 는 base_footprint 기준 앞(+)
    ("차체", 4.5, 0.00, 0.08),
    ("로봇팔", 1.0, 0.06, 0.30),
    ("물건", 0.25, 0.30, 0.25),
]
m = np.array([p[1] for p in parts])
x = np.array([p[2] for p in parts])
z = np.array([p[3] for p in parts])
cx = 0.0   # TODO: 질량 가중 평균  sum(m * x) / sum(m)
cz = 0.0   # TODO: 같은 방법으로 z
print("전체 질량 %.2f kg, 무게중심 x=%.3f m, z=%.3f m" % (m.sum(), cx, cz))
front_axle = 0.085
print("앞바퀴 축까지 여유 %.3f m" % (front_axle - cx))
풀이 코드 보기
python시뮬 실행 가능
import numpy as np

# 선반 통로의 물체: (이름, 아래 높이 m, 위 높이 m)
OBJECTS = [
    ("팔레트", 0.00, 0.12),
    ("낮은 상자", 0.00, 0.25),
    ("높은 상자", 0.00, 0.40),
    ("작업대 상판", 0.45, 0.50),
]

def detected(h, z_lo, z_hi):
    """2D LiDAR 는 높이 h 의 수평면 하나만 봅니다."""
    return z_lo <= h < z_hi

# 1) 높이별로 무엇이 보이나
for h in (0.10, 0.20, 0.30, 0.47):
    seen = [n for n, lo, hi in OBJECTS if detected(h, lo, hi)]
    print("LiDAR %.2f m : %s" % (h, ", ".join(seen) if seen else "아무것도 안 보임"))

# 2) 낮은 상자가 잡히는 최대 높이 (0.01 m 간격으로 찾기)
heights = np.round(np.arange(0.10, 0.501, 0.01), 2)
ok = [h for h in heights if detected(h, 0.00, 0.25)]
print("낮은 상자가 잡히는 최대 LiDAR 높이:", max(ok) if ok else "없음", "m")

# 3) 로봇팔을 얹었을 때 무게중심 (질량 · 위치는 교육용 가정 - 확인 필요)
parts = [  # (이름, 질량 kg, x m, z m)  x 는 base_footprint 기준 앞(+)
    ("차체", 4.5, 0.00, 0.08),
    ("로봇팔", 1.0, 0.06, 0.30),
    ("물건", 0.25, 0.30, 0.25),
]
m = np.array([p[1] for p in parts])
x = np.array([p[2] for p in parts])
z = np.array([p[3] for p in parts])
cx = (m * x).sum() / m.sum()
cz = (m * z).sum() / m.sum()
print("전체 질량 %.2f kg, 무게중심 x=%.3f m, z=%.3f m" % (m.sum(), cx, cz))
front_axle = 0.085
print("앞바퀴 축까지 여유 %.3f m" % (front_axle - cx))

실행 결과: 0.10 m 에서는 팔레트 · 낮은 상자 · 높은 상자, 0.20 m 에서는 상자 두 개, 0.30 m 에서는 높은 상자만, 0.47 m 에서는 작업대 상판만 보입니다. 낮은 상자의 최대 높이는 0.24 m 입니다. 무게중심은 x = 0.023 m, z = 0.126 m 로, 차체만 있을 때(0, 0.08)보다 앞 · 위로 옮겨졌습니다. 물건의 x 를 0.30 → 0.60 으로 바꿔 여유가 얼마나 줄어드는지 확인해 보세요.

확인 문제 4-1

시뮬레이터 5 에서 LiDAR 높이를 올리며 낮은 상자(높이 0.25 m)를 관찰했습니다. 상자가 스캔에 잡히는 최대 LiDAR 높이(0.01 m 단위)는?

  1. 0.12 m
  2. 0.24 m
  3. 0.26 m
  4. 0.40 m
정답 보기② — 2D LiDAR 는 높이 h 의 수평면만 보므로 h 가 상자 높이(0.25 m)보다 낮아야 합니다. 0.12 m 는 팔레트가 사라지는 높이, 0.40 m 는 높은 상자의 높이입니다.
확인 문제 4-2

LiDAR 관절을 <origin xyz="0.065 0 0.16" rpy="0 0 180"/> 로 적었더니 ROS 는 오류 없이 실행되었지만 RViz 에서 스캔이 엉뚱한 방향으로 돌아가 보였습니다. 원인은?

  1. xyz 를 mm 로 적었다
  2. rpy 를 도(degree)로 적었다 — URDF 는 rad 이므로 3.14159 로 적어야 한다
  3. package:// 경로를 풀지 못했다
  4. 부모 링크 이름이 틀렸다
정답 보기② — 180 은 rad 로 해석되어 약 233° 회전이 됩니다. XML 문법상 문제가 없으므로 오류 메시지가 나오지 않는 것이 이 실수의 무서운 점입니다. ④ 였다면 트리를 만들 수 없다는 오류가 났을 것입니다.
확인 문제 4-3

시뮬레이터 5 에서 로봇팔 얹기를 켜자, LiDAR 높이(0.16 m)는 그대로인데 작업대 상판의 판정이 "아래로 지나감" 에서 "충돌 위험" 으로 바뀌었습니다. 이유로 가장 알맞은 것은?

  1. 로봇팔이 LiDAR 를 가려서 상판이 안 보이게 되었다
  2. 로봇의 높이가 상판 아래 높이(0.45 m)보다 커졌는데, 상판은 스캔 평면보다 위에 있어 여전히 보이지 않기 때문이다
  3. 로봇팔의 무게 때문에 LiDAR 가 아래로 기울었다
  4. 작업대 다리가 더 이상 보이지 않게 되었다
정답 보기② — 센서가 보는 높이와 로봇이 차지하는 높이는 별개입니다. 무언가를 위에 얹으면 그 높이에 걸리는 물체를 볼 수단(카메라 · 깊이 센서 · 금지 구역)이 따로 필요합니다.

실습 (실물)#

로봇에서 TF 트리를 직접 확인하는 선택 실습입니다. 로봇은 움직이지 않고 노드만 띄웁니다. 명령과 패키지 이름은 myagv_ros2 저장소 기준이며, myAGV Plus 출고 이미지의 패키지 구성은 확인 필요입니다 (ROS 2 환경 준비는 ch09).

실물 확인 전 점검
bash
# 터미널 1: 오도메트리 + 정적 TF + LiDAR 드라이버 실행 (myagv_ros2 기준)
ros2 launch myagv_odometry myagv_active.launch.py

# 터미널 2: TF 트리를 PDF 로 저장 (현재 폴더에 frames_*.pdf 생성)
ros2 run tf2_tools view_frames

# 터미널 2: 두 프레임 사이 변환을 계속 출력 (Ctrl+C 로 종료)
ros2 run tf2_ros tf2_echo base_footprint laser_frame
ros2 run tf2_ros tf2_echo base_footprint imu_link
실물 확인 과제
  1. view_frames 의 PDF 에서 base_footprint 의 자식 프레임을 모두 적고, 이 차시의 교육용 트리 그림과 비교합니다.
  2. tf2_echo base_footprint laser_frame 의 Translation 과 Rotation 값을 기록합니다. Rotation 의 RPY(도) 값이 yaw 180° 에 해당하는지 확인합니다.
  3. tf2_echo base_footprint imu_link 의 RPY 가 (180, 180, 0) 으로 나오나요, (0, 0, 180) 으로 나오나요? 두 표현이 같은 회전이라는 것을 실습 과제 2-1 로 설명합니다.

자주 나는 오류와 해결#

1) 메시를 찾지 못해 로봇이 보이지 않음 (package:// 경로)#

text
[rviz2]: Could not load resource [package://myagv_description/meshes/myagv_base.dae]: Unable to open file "package://myagv_description/meshes/myagv_base.dae".
  • 원인: package://패키지/... 는 ROS 가 패키지 설치 위치를 찾아 바꿔 주는 주소입니다. 패키지를 빌드 · source 하지 않았거나, 웹 3D 뷰어처럼 ROS 가 없는 곳에서 열면 풀리지 않습니다. (메시지 형식은 버전에 따라 다를 수 있습니다.)
  • 해결: ROS 에서는 colcon build 후 source install/setup.bash 를 했는지 확인합니다. 이 강좌의 URDF 뷰어용 모델은 메시 경로를 URDF 파일 기준 상대 경로(meshes/x.glb)로 바꿉니다.

2) 센서가 수십 m 밖에 떠 있음 (mm 단위)#

text
(오류 메시지 없음) RViz 에서 laser_frame 이 로봇에서 65 m 떨어진 곳에 표시됨
  • 원인: CAD 도면의 mm 값을 그대로 xyz="65 0 160" 으로 적었습니다. URDF 의 길이 단위는 m 입니다.
  • 해결: 1000 으로 나눠 xyz="0.065 0 0.16" 으로 고칩니다. 실습 과제 4-1 의 검사기처럼 "2 m 넘는 장착 위치" 를 의심하는 습관을 들입니다.

3) 스캔이 엉뚱한 방향으로 돌아가 보임 (rpy 를 도로 입력)#

text
(오류 메시지 없음) LiDAR 점들이 실제 벽과 233° 어긋나게 그려짐
  • 원인: rpy="0 0 180" 처럼 도(degree)를 적었습니다. URDF · TF 의 각도는 rad 이므로 180 rad(≈ 233°)로 해석됩니다.
  • 해결: rpy="0 0 3.14159" (180° = π rad, 90° = 1.5708 rad) 로 고칩니다. static_transform_publisher 를 쓸 때는 인자 순서(yaw pitch roll) 도 함께 확인합니다.

4) URDF 를 읽지 못함: 부모 링크 없음 · 뿌리가 둘#

text
Failed to build tree: parent link [base_foot_print] of joint [laser_joint] not found.  This is not valid according to the URDF spec.
Failed to build tree: Two root links found: [base_footprint] and [arm_base]
  • 원인: 관절의 <parent link> 이름에 오타가 있거나, 로봇팔 URDF 를 붙이면서 연결 관절을 빠뜨려 트리가 두 개로 나뉘었습니다.
  • 해결: 링크 이름을 복사해서 붙여 넣고, 모든 링크가 하나의 뿌리에 이어지는지 실습 과제 1-1 의 show_tree() 로 확인합니다. 팔을 얹을 때는 arm_mount_joint 같은 fixed 관절을 추가합니다.

5) TF 를 찾지 못함 (프레임 이름 불일치)#

text
"laser" passed to lookupTransform argument source_frame does not exist.
  • 원인: 코드나 설정에 laser 라고 적었는데, 드라이버는 laser_frame (ydlidar 파라미터의 frame_id) 으로 데이터를 보냅니다. 또는 정적 TF 노드를 띄우지 않았습니다.
  • 해결: ros2 run tf2_tools view_frames 로 실제 프레임 이름을 확인하고, 센서 드라이버의 frame_id · URDF 링크 이름 · 코드의 이름을 하나로 맞춥니다.

6) Playground: 행렬 곱 결과가 이상함#

text
ValueError: matmul: Input operand 1 has a mismatch in its core dimension 0, with gufunc signature (n?,k),(k,m?)->(n?,m?) (size 2 is different from 3)
  • 원인: 3×3 동차 행렬에 (x, y) 두 값짜리 점을 곱했습니다. 또는 * (원소별 곱)로 행렬 곱을 하려 했습니다.
  • 해결: 점 끝에 1 을 붙여 np.array([x, y, 1.0]) 로 만들고, 행렬 곱은 @ 를 씁니다. 결과가 지도 밖으로 튀면 곱하는 순서(아래 첨자 맞닿기)를 확인합니다.

과제#

  1. 트리 그리기: 교육용 모델에 "상판 위 로봇팔(arm_base, 링크 6개 가정)" 과 "전면 범퍼 센서(bumper_link)" 를 추가한 트리를 손으로 그리고, 새 관절마다 type 과 부모를 적으세요.
  2. 변환 표: 로봇 (1.5, 2.0, −45°) 에서 LiDAR(yaw 180°) 가 α = 0°, 90°, 180° 방향 1 m 에 점을 보았습니다. 세 점의 지도 좌표를 손으로 하나, 실습 과제 3-1 코드로 셋 모두 구해 표로 정리하세요.
  3. 센서 배치 보고서: 시뮬레이터 5 · 6 의 결과를 근거로, 팔레트가 많은 창고에서 쓸 LiDAR 높이를 하나 정하고 그 높이에서 놓치는 위험과 보완 방법을 5줄 이내로 쓰세요.
팀 미션 · Part 1 마무리: "우리 로봇의 좌표 카드"

3~4명이 한 팀이 되어 다음을 한 장(A4)으로 만듭니다.

  1. 트리: 교육용 myAGV Plus 트리 + 팀이 정한 추가 장비(로봇팔 · 바구니 · 추가 센서) 1개 이상을 넣은 링크 · 관절 트리
  2. 좌표 표: 모든 센서의 base_footprint 기준 xyz · rpy (출처 표기: 저장소 값 / 교육용 가정 / 실측)
  3. 검증: 시뮬레이터 4 와 Playground 로 "LiDAR 가 정면 1 m 에서 본 점" 의 지도 좌표를 각각 구해 일치하는 화면을 캡처
  4. 위험 목록: 시뮬레이터 5 에서 찾은 "보이지 않는 장애물" 2가지와 팀의 대책

발표 3분: 다른 팀은 상대 팀의 좌표 표에서 단위 · 부호 · 회전 순서 실수를 하나 이상 찾아 질문합니다.

참고자료#

  • myAGV Plus 공식 문서 (영문): https://github.com/elephantrobotics/myagvPlus_docs/tree/main/MYAGV_PLUS_EN
  • myagv_ros2 저장소: https://github.com/elephantrobotics/myagv_ros2
    • URDF: myagv_description/urdf/myAGV.urdf
    • 정적 TF (카메라 · IMU): myagv_odometry/launch/myagv_active.launch.py
    • 정적 TF (LiDAR) · 파라미터: ydlidar_ros2_driver/launch/ydlidar_launch.py, ydlidar_ros2_driver/params/X2.yaml
  • myAGV Plus 제품 사진 · 소개 (hyenc.net): https://www.hyenc.net/products/35
  • REP-103 표준 단위 · 좌표 규칙: https://www.ros.org/reps/rep-0103.html
  • REP-105 이동 로봇 좌표 프레임 (map · odom · base_link): https://www.ros.org/reps/rep-0105.html
  • URDF 형식 (ROS Wiki): https://wiki.ros.org/urdf/XML
  • ROS 2 Humble tf2 튜토리얼: https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Tf2-Main.html
  • 이 사이트의 도구: URDF 뷰어 · Python Playground
  • 이전 차시: ch01 myAGV Plus 와 자율주행 물류로봇 · 다음 차시: ch03 메카넘 휠과 전방향 운동학