[Physical AI W2D2] 7/7 — MoveIt 경로계획 RViz2 시각화 실습(+ 오류 점검 7종)

[Physical AI W2D2 · 7/7]

실제 move_group을 띄우지 않고, MoveIt 경로 계획의 핵심 개념(Planning Frame·좌표계 체인·End-Effector·Target Pose·Planned Trajectory·Motion Execution)을 ROS 2 TF와 RViz2 Marker로 직접 시각화한다.
7관절 좌표계 체인을 TF로 발행하고, 전진기구학으로 tool0 위치를 계산해 화면에서 로봇 팔이 시작 자세와 목표 자세 사이를 반복 이동하는 것을 확인한다. 마지막에 실습 중 자주 겪는 오류 7종을 증상→원인→해결로 정리한다.

이 글에서 직접 해보는 것

1. 본 주제 전용 작업공간 ~/moveit_planning_ws + 패키지 moveit_planning_demo 생성
2. 로봇 팔 좌표계 체인(world → base_link → ... → tool0)을 TF로 발행
3. 전진기구학(동차 변환 행렬 곱) 으로 tool0 위치를 계산하고 RViz2 Marker로 표시
4. moveit_planning_visualizer.py(약 600줄) — 관절 보간·FK·TF·MarkerArray를 발행하는 노드
5. RViz2에서 Planning Frame·End-Effector·Target Pose·Planned Trajectory·Motion Execution 확인
6. 오류 점검 7종(RViz 창 / Marker / TF / Fixed Frame / ros2 run / 코드 미반영 / 토픽 섞임)
   
   

(5·6편에서 MoveIt 개념을 잡았다면, 이제 그 개념을 눈으로 확인합니다. 3편의 헤드리스 RViz 스택(Xvfb→x11vnc→noVNC→Cloudflared)과 4편의 FK→RViz 흐름이 그대로 이어집니다.)

---

들어가며 — move_group 없이 MoveIt "개념"을 먼저 본다

MoveIt은 로봇 팔의 경로 계획, 목표 자세 도달, 충돌 회피, 관절 제어를 지원하는 ROS 기반 로봇 조작 프레임워크입니다.

실제 MoveIt 환경에서는 로봇 모델(URDF/SRDF), 관절 상태, 목표 Pose, Planning Scene, 충돌 검사, 경로 계획 알고리즘, Controller가 함께 동작합니다.

 

이번 실습은 실제 MoveIt의 move_group을 실행하는 실습이 아닙니다. 대신 MoveIt이 내부적으로 사용하는 좌표계, 로봇 팔 링크 구조, End-Effector 위치, 목표점, 계획 경로를 ROS 2 TF와 RViz2 Marker로 시각화합니다.

산업용 로봇 팔의 전체 MoveIt 패키지를 구성하기 전에, 경로 계획에서 반드시 이해해야 하는 핵심 개념을 먼저 화면으로 잡는 단계입니다.

 

이번 실습에서 다루는 핵심 개념은 다음과 같습니다.

Planning Frame
Robot Base Frame
Joint Frame
Link Frame
End-Effector Frame
Current Robot State
Target Pose
Planned Trajectory
Forward Kinematics
Homogeneous Transformation Matrix
Motion Execution

실습의 최종 목표는 다음 구조를 RViz2에서 확인하는 것입니다.

world
└── base_link
    └── shoulder_link
        └── upper_arm_link
            └── elbow_link
                └── forearm_link
                    └── wrist_link
                        └── tool0

tool0는 로봇 팔 끝단 좌표계이며, MoveIt에서 목표 Pose를 지정할 때 핵심이 되는 End-Effector Frame입니다.

💡 터미널 역할 분리 — 3편과 동일하게 SSH 터미널을 나눕니다.
터미널 1: RViz2 실행,
터미널 2: Visualizer 노드 실행,
터미널 3: 토픽/TF 확인. 노드가 동시에 돌기 때문입니다.

---

1. 실습 목표

본 실습의 목표는 다음과 같습니다.

기존에 실행 중인 ROS 2/RViz2 실습을 중지할 수 있다.
본 주제 전용 ROS 2 작업공간을 생성할 수 있다.
MoveIt 경로 계획 개념을 RViz2 시각화와 연결하여 이해할 수 있다.
로봇 팔의 좌표계 체인을 TF로 발행할 수 있다.
RViz2에서 world, base_link, shoulder_link, upper_arm_link, elbow_link, forearm_link, wrist_link, tool0 좌표계를 확인할 수 있다.
RViz2 Marker로 로봇 팔 링크, 관절, End-Effector, 목표 Pose, 계획 경로를 확인할 수 있다.
현재 End-Effector 위치와 목표 Pose의 차이를 확인할 수 있다.
동차 변환 행렬이 로봇 팔 전진기구학 계산에 사용되는 방식을 이해할 수 있다.
MoveIt의 경로 계획과 제어 흐름을 단순화된 시각화 실습으로 이해할 수 있다.

---

2. 실습 전제 — 환경 확인

본 실습은 ROS 2와 RViz2가 이미 설치되어 있는 GCP VM 환경을 기준으로 진행합니다.

설치 과정은 다루지 않습니다(3편·4편 참고).

다음 환경이 이미 준비되어 있다고 가정합니다.

GCP VM Ubuntu 22.04
ROS 2 Humble
RViz2
colcon
Python 3.10
브라우저 noVNC 화면 접속 환경
Cloudflared quick tunnel

설치 명령은 포함하지 않습니다. 현재 환경만 확인합니다.

source /opt/ros/humble/setup.bash
ros2 --help
which rviz2
which colcon

정상 출력 예시는 다음과 같습니다.

/opt/ros/humble/bin/rviz2
/usr/bin/colcon

RViz2 화면 전달 구성이 이미 실행 중인지 확인합니다.

ps -ef | grep -E "Xvfb|fluxbox|x11vnc|websockify|cloudflared" | grep -v grep

Cloudflared URL을 확인합니다.

tail -n 20 ~/tunnel.log

브라우저 접속 주소 형식은 다음과 같습니다.

https://xxxx-xxxx-xxxx.trycloudflare.com/vnc.html

💡 헤드리스 RViz 스택은 3편에서 — Xvfb(가상화면) → x11vnc(VNC 공유) → noVNC/websockify(웹 변환) → Cloudflared(외부 URL) 구성은 3편에서 자세히 다뤘습니다.
여기서는 그 스택이 살아 있다고 전제하고, RViz2와 ROS 2 노드만 새로 실행합니다.

---

3. 기존 실습 실행 중지

기존에 실행 중인 RViz2 실습이 있으면 먼저 중지합니다. 이전 실습의 Publisher 노드, TF Publisher, Marker Publisher, MoveIt 관련 프로세스가 남아 있으면 /tf, Marker, LaserScan, Odometry, Path 등이 새 실습과 섞일 수 있습니다.

무엇을 — 기존 ROS 2 노드 중지. 왜 — 토픽·TF가 섞이는 것을 방지. 기대결과 — 관련 프로세스가 모두 사라짐.

기존 ROS 2 노드를 중지합니다.

pkill -f "sensor_demo_publisher" || true
pkill -f "arm_frame_visualizer" || true
pkill -f "moveit_planning_visualizer" || true
pkill -f "robot_state_publisher" || true
pkill -f "joint_state_publisher" || true
pkill -f "move_group" || true
pkill -f "static_transform_publisher" || true

기존 RViz2 실행을 중지합니다.

pkill -f "rviz2" || true

현재 남아 있는 관련 프로세스를 확인합니다.

ps -ef | grep -E "rviz2|sensor_demo_publisher|arm_frame_visualizer|moveit_planning_visualizer|robot_state_publisher|joint_state_publisher|move_group|static_transform_publisher" | grep -v grep

아무 출력이 없으면 기존 ROS 2 실습 프로세스가 정리된 것입니다. 화면 공유 프로세스는 정상 동작 중이면 유지합니다.

Xvfb
fluxbox
x11vnc
websockify
cloudflared

이 프로세스들은 RViz2 화면을 브라우저에서 확인하기 위한 구성입니다. 본 실습에서는 RViz2와 ROS 2 노드만 새로 실행합니다.

---

4. ROS 2 토픽 정리 확인

기존 실습 토픽이 남아 있는지 확인합니다.

source /opt/ros/humble/setup.bash
ros2 topic list

기존 센서 시각화 실습이 종료되었다면 다음 토픽은 더 이상 보이지 않아야 합니다.

/demo/scan
/demo/odom
/demo/path

ROS 2 데몬 상태 때문에 이전 토픽이 잠시 남아 보이면 데몬을 재시작합니다.

ros2 daemon stop
ros2 daemon start
ros2 topic list

본 실습에서 사용할 주요 토픽은 다음입니다.

/tf
/moveit_demo/markers

---

5. 본 주제 작업공간 생성

기존 RViz 센서 실습과 구분하기 위해 본 실습 전용 작업공간을 사용합니다.

mkdir -p ~/moveit_planning_ws/src
cd ~/moveit_planning_ws/src

현재 경로를 확인합니다.

pwd

예상 경로는 다음과 같습니다.

/home/사용자명/moveit_planning_ws/src

ROS 2 환경을 불러옵니다.

source /opt/ros/humble/setup.bash

---

6. ROS 2 패키지 생성

MoveIt 경로 계획 개념 시각화용 ROS 2 Python 패키지를 생성합니다.

ros2 pkg create moveit_planning_demo \
  --build-type ament_python \
  --dependencies rclpy geometry_msgs visualization_msgs std_msgs tf2_ros

생성된 파일을 확인합니다.

find ~/moveit_planning_ws/src/moveit_planning_demo -maxdepth 3 -type f | sort

예상되는 주요 파일은 다음과 같습니다.

/home/사용자명/moveit_planning_ws/src/moveit_planning_demo/package.xml
/home/사용자명/moveit_planning_ws/src/moveit_planning_demo/setup.py
/home/사용자명/moveit_planning_ws/src/moveit_planning_demo/moveit_planning_demo/__init__.py

💡 의존성 5종 —
rclpy(파이썬 노드),
geometry_msgs(Point·TransformStamped),
visualization_msgs(Marker·MarkerArray),
std_msgs(ColorRGBA),
tf2_ros(TransformBroadcaster).
노드 코드에서 import할 메시지 타입들입니다.

---

7. 본 실습 실행 구조

본 실습은 이전 RViz 센서 시각화 실습과 목적이 다릅니다. 이전 센서 시각화 실습의 주요 토픽은 다음이었습니다.

/demo/scan
/demo/odom
/demo/path

본 실습의 주요 토픽은 다음입니다.

/tf
/moveit_demo/markers

각 토픽의 의미는 다음과 같습니다.

/tf
→ 로봇 팔의 좌표계 체인을 발행한다.

/moveit_demo/markers
→ 로봇 팔 링크, 관절, End-Effector, 목표 Pose, 계획 경로를 RViz2에 표시한다.

RViz2의 Fixed Frame은 다음으로 설정합니다.

world

이번 실습의 좌표계 체인은 다음과 같습니다.

world
└── base_link
    └── shoulder_link
        └── upper_arm_link
            └── elbow_link
                └── forearm_link
                    └── wrist_link
                        └── tool0

💡 MoveIt 개념 ↔ 실습 대응 — 실제 MoveIt은 현재 관절 상태를 기반으로 End-Effector의 현재 Pose를 계산하고, 목표 Pose까지 도달 가능한 경로를 계획합니다.
본 실습에서는 이 과정을 단순화하여 RViz2에서 시각적으로 확인합니다.

---

8. RViz2 설정 파일 생성

이전 센서 실습에서 사용한 RViz 설정 파일을 그대로 사용하지 않습니다.

LaserScan, Odometry, Path Display가 남아 있으면 본 주제와 맞지 않기 때문입니다.

로봇 팔 경로 계획 개념 실습용 RViz2 설정 파일을 새로 생성합니다.

mkdir -p ~/moveit_planning_ws/rviz
nano ~/moveit_planning_ws/rviz/moveit_planning_demo.rviz

다음 내용을 입력합니다.

Panels:
  - Class: rviz_common/Displays
    Name: Displays
  - Class: rviz_common/Views
    Name: Views

Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz_default_plugins/Grid
      Color: 160; 160; 164
      Enabled: true
      Name: Grid
      Plane: XY
      Plane Cell Count: 20
      Reference Frame: <Fixed Frame>
      Value: true

    - Class: rviz_default_plugins/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: true
      Marker Scale: 0.45
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Update Interval: 0
      Value: true

    - Class: rviz_default_plugins/MarkerArray
      Enabled: true
      Name: MoveIt Planning Markers
      Topic:
        Value: /moveit_demo/markers
      Value: true

  Enabled: true

  Global Options:
    Background Color: 48; 48; 48
    Fixed Frame: world
    Frame Rate: 30

  Name: root

  Tools:
    - Class: rviz_default_plugins/Interact
    - Class: rviz_default_plugins/MoveCamera
    - Class: rviz_default_plugins/Select
    - Class: rviz_default_plugins/FocusCamera

  Value: true

  Views:
    Current:
      Class: rviz_default_plugins/Orbit
      Distance: 4
      Focal Point:
        X: 0.6
        Y: 0
        Z: 0.6
      Name: Current View
      Pitch: 0.6
      Target Frame: <Fixed Frame>
      Yaw: 0.8

파일을 저장합니다.

Ctrl + O
Enter
Ctrl + X

이 설정 파일에는 다음 Display가 포함됩니다.

Grid
TF
MarkerArray

요지는 Fixed Frame world(모든 데이터의 기준) + 3개 Display — Grid·TF·MarkerArray(/moveit_demo/markers) 입니다.

⚠️ Fixed Frame은 반드시 world — 이번 실습의 모든 Marker는 frame_id: world로, 모든 TF는 world를 루트로 발행됩니다.
Fixed Frame이 map(RViz 기본값)으로 떨어지면 화면에 아무것도 안 보입니다(오류 점검 4 참고).

---

9. 로봇 팔 좌표계 구조 설계

본 실습에서 사용할 로봇 팔은 교육용 단순 모델입니다. 실제 URDF 기반 로봇 모델은 아니지만, MoveIt의 경로 계획 개념을 이해하기 위해 필요한 좌표계 구조를 갖습니다.

좌표계 구조는 다음과 같습니다.

world
└── base_link
    └── shoulder_link
        └── upper_arm_link
            └── elbow_link
                └── forearm_link
                    └── wrist_link
                        └── tool0

각 좌표계의 의미는 다음과 같습니다.

world
→ 전체 기준 좌표계이며 RViz2의 Fixed Frame이다.

base_link
→ 로봇 팔이 설치된 기준 좌표계이다.

shoulder_link
→ 어깨 관절의 회전 기준 좌표계이다.

upper_arm_link
→ 어깨에서 팔꿈치까지 이어지는 상완 링크 좌표계이다.

elbow_link
→ 팔꿈치 관절의 회전 기준 좌표계이다.

forearm_link
→ 팔꿈치에서 손목까지 이어지는 전완 링크 좌표계이다.

wrist_link
→ 손목 관절의 회전 기준 좌표계이다.

tool0
→ 로봇 팔 끝단 좌표계이며 End-Effector Frame이다.

MoveIt 관점에서 tool0는 목표 Pose를 지정하는 핵심 좌표계입니다.

---

10. 로봇 팔 경로 계획 시각화 노드 작성

이번 실습의 핵심입니다. 노드 파일을 생성합니다.

cd ~/moveit_planning_ws/src/moveit_planning_demo/moveit_planning_demo
nano moveit_planning_visualizer.py

다음 코드를 한 글자도 바꾸지 말고 전체 입력합니다(들여쓰기 포함). 코드 구조 설명은 11~16절에서 이어집니다.

import math

import rclpy
from rclpy.node import Node

from geometry_msgs.msg import Point, TransformStamped
from std_msgs.msg import ColorRGBA
from tf2_ros import TransformBroadcaster
from visualization_msgs.msg import Marker, MarkerArray


class MoveItPlanningVisualizer(Node):
    def __init__(self):
        super().__init__('moveit_planning_visualizer')

        self.tf_broadcaster = TransformBroadcaster(self)
        self.marker_pub = self.create_publisher(
            MarkerArray,
            '/moveit_demo/markers',
            10
        )

        self.execution_ratio = 0.0
        self.execution_direction = 1.0

        self.timer = self.create_timer(0.05, self.publish_demo)

        self.get_logger().info('MoveIt planning concept visualizer started')

    def publish_demo(self):
        stamp = self.get_clock().now().to_msg()

        start_joints = [0.2, 0.3, -0.4]
        goal_joints = [1.0, -0.5, 0.6]

        self.execution_ratio += 0.01 * self.execution_direction

        if self.execution_ratio >= 1.0:
            self.execution_ratio = 1.0
            self.execution_direction = -1.0

        if self.execution_ratio <= 0.0:
            self.execution_ratio = 0.0
            self.execution_direction = 1.0

        q1 = self.interpolate(
            start_joints[0],
            goal_joints[0],
            self.execution_ratio
        )
        q2 = self.interpolate(
            start_joints[1],
            goal_joints[1],
            self.execution_ratio
        )
        q3 = self.interpolate(
            start_joints[2],
            goal_joints[2],
            self.execution_ratio
        )

        link1 = 0.75
        link2 = 0.55
        link3 = 0.35

        current_points = self.forward_kinematics(
            q1,
            q2,
            q3,
            link1,
            link2,
            link3
        )

        start_points = self.forward_kinematics(
            start_joints[0],
            start_joints[1],
            start_joints[2],
            link1,
            link2,
            link3
        )

        goal_points = self.forward_kinematics(
            goal_joints[0],
            goal_joints[1],
            goal_joints[2],
            link1,
            link2,
            link3
        )

        start_tool = start_points[-1]
        current_tool = current_points[-1]
        goal_tool = goal_points[-1]

        planned_path = self.generate_planned_path(start_tool, goal_tool)

        self.publish_tf(stamp, q1, q2, q3, link1, link2, link3)
        self.publish_markers(
            stamp,
            current_points,
            start_tool,
            current_tool,
            goal_tool,
            planned_path
        )

    def interpolate(self, start, goal, ratio):
        return start + (goal - start) * ratio

    def forward_kinematics(self, q1, q2, q3, link1, link2, link3):
        t_world_base = self.make_transform(
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0
        )

        t_base_shoulder = self.make_transform(
            0.0,
            0.0,
            0.4,
            0.0,
            0.0,
            q1
        )

        t_shoulder_upper = self.make_transform(
            link1,
            0.0,
            0.0,
            0.0,
            q2,
            0.0
        )

        t_upper_elbow = self.make_transform(
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0
        )

        t_elbow_forearm = self.make_transform(
            link2,
            0.0,
            0.0,
            0.0,
            q3,
            0.0
        )

        t_forearm_wrist = self.make_transform(
            link3,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0
        )

        t_wrist_tool = self.make_transform(
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0
        )

        t_world_shoulder = self.matmul(
            t_world_base,
            t_base_shoulder
        )

        t_world_upper = self.matmul(
            t_world_shoulder,
            t_shoulder_upper
        )

        t_world_elbow = self.matmul(
            t_world_upper,
            t_upper_elbow
        )

        t_world_forearm = self.matmul(
            t_world_elbow,
            t_elbow_forearm
        )

        t_world_wrist = self.matmul(
            t_world_forearm,
            t_forearm_wrist
        )

        t_world_tool = self.matmul(
            t_world_wrist,
            t_wrist_tool
        )

        base_pos = self.translation_from_matrix(t_world_base)
        shoulder_pos = self.translation_from_matrix(t_world_shoulder)
        upper_pos = self.translation_from_matrix(t_world_upper)
        elbow_pos = self.translation_from_matrix(t_world_elbow)
        forearm_pos = self.translation_from_matrix(t_world_forearm)
        wrist_pos = self.translation_from_matrix(t_world_wrist)
        tool_pos = self.translation_from_matrix(t_world_tool)

        return [
            base_pos,
            shoulder_pos,
            upper_pos,
            elbow_pos,
            forearm_pos,
            wrist_pos,
            tool_pos
        ]

    def generate_planned_path(self, start_tool, goal_tool):
        path = []

        for i in range(31):
            ratio = i / 30.0

            x = self.interpolate(
                start_tool[0],
                goal_tool[0],
                ratio
            )
            y = self.interpolate(
                start_tool[1],
                goal_tool[1],
                ratio
            )
            z = self.interpolate(
                start_tool[2],
                goal_tool[2],
                ratio
            )

            lift = 0.25 * math.sin(math.pi * ratio)
            z = z + lift

            path.append((x, y, z))

        return path

    def publish_tf(self, stamp, q1, q2, q3, link1, link2, link3):
        transforms = [
            self.make_tf(
                stamp,
                'world',
                'base_link',
                0.0,
                0.0,
                0.0,
                0.0,
                0.0,
                0.0
            ),
            self.make_tf(
                stamp,
                'base_link',
                'shoulder_link',
                0.0,
                0.0,
                0.4,
                0.0,
                0.0,
                q1
            ),
            self.make_tf(
                stamp,
                'shoulder_link',
                'upper_arm_link',
                link1,
                0.0,
                0.0,
                0.0,
                q2,
                0.0
            ),
            self.make_tf(
                stamp,
                'upper_arm_link',
                'elbow_link',
                0.0,
                0.0,
                0.0,
                0.0,
                0.0,
                0.0
            ),
            self.make_tf(
                stamp,
                'elbow_link',
                'forearm_link',
                link2,
                0.0,
                0.0,
                0.0,
                q3,
                0.0
            ),
            self.make_tf(
                stamp,
                'forearm_link',
                'wrist_link',
                link3,
                0.0,
                0.0,
                0.0,
                0.0,
                0.0
            ),
            self.make_tf(
                stamp,
                'wrist_link',
                'tool0',
                0.0,
                0.0,
                0.0,
                0.0,
                0.0,
                0.0
            )
        ]

        for tf_msg in transforms:
            self.tf_broadcaster.sendTransform(tf_msg)

    def publish_markers(
        self,
        stamp,
        current_points,
        start_tool,
        current_tool,
        goal_tool,
        planned_path
    ):
        marker_array = MarkerArray()

        marker_array.markers.append(
            self.make_line_marker(
                stamp,
                0,
                'robot_arm_links',
                current_points,
                0.05,
                self.color(0.1, 0.8, 1.0, 1.0)
            )
        )

        marker_id = 1
        for point in current_points:
            marker_array.markers.append(
                self.make_sphere_marker(
                    stamp,
                    marker_id,
                    'robot_arm_joints',
                    point,
                    0.08,
                    self.color(1.0, 0.6, 0.1, 1.0)
                )
            )
            marker_id += 1

        marker_array.markers.append(
            self.make_sphere_marker(
                stamp,
                20,
                'start_tool_pose',
                start_tool,
                0.11,
                self.color(0.2, 1.0, 0.2, 1.0)
            )
        )

        marker_array.markers.append(
            self.make_sphere_marker(
                stamp,
                21,
                'current_tool_pose',
                current_tool,
                0.12,
                self.color(1.0, 0.1, 0.1, 1.0)
            )
        )

        marker_array.markers.append(
            self.make_sphere_marker(
                stamp,
                22,
                'target_pose',
                goal_tool,
                0.13,
                self.color(0.9, 0.1, 1.0, 1.0)
            )
        )

        marker_array.markers.append(
            self.make_line_marker(
                stamp,
                23,
                'planned_trajectory',
                planned_path,
                0.03,
                self.color(1.0, 1.0, 0.1, 1.0)
            )
        )

        marker_array.markers.append(
            self.make_line_marker(
                stamp,
                24,
                'tool_to_target',
                [current_tool, goal_tool],
                0.015,
                self.color(1.0, 0.2, 0.2, 0.8)
            )
        )

        self.marker_pub.publish(marker_array)

    def make_tf(
        self,
        stamp,
        parent,
        child,
        x,
        y,
        z,
        roll,
        pitch,
        yaw
    ):
        tf_msg = TransformStamped()
        tf_msg.header.stamp = stamp
        tf_msg.header.frame_id = parent
        tf_msg.child_frame_id = child

        tf_msg.transform.translation.x = x
        tf_msg.transform.translation.y = y
        tf_msg.transform.translation.z = z

        qx, qy, qz, qw = self.rpy_to_quaternion(
            roll,
            pitch,
            yaw
        )

        tf_msg.transform.rotation.x = qx
        tf_msg.transform.rotation.y = qy
        tf_msg.transform.rotation.z = qz
        tf_msg.transform.rotation.w = qw

        return tf_msg

    def make_line_marker(
        self,
        stamp,
        marker_id,
        name,
        points,
        width,
        color
    ):
        marker = Marker()
        marker.header.frame_id = 'world'
        marker.header.stamp = stamp
        marker.ns = name
        marker.id = marker_id
        marker.type = Marker.LINE_STRIP
        marker.action = Marker.ADD
        marker.scale.x = width
        marker.color = color

        marker.points = []

        for p in points:
            point = Point()
            point.x = float(p[0])
            point.y = float(p[1])
            point.z = float(p[2])
            marker.points.append(point)

        return marker

    def make_sphere_marker(
        self,
        stamp,
        marker_id,
        name,
        position,
        size,
        color
    ):
        marker = Marker()
        marker.header.frame_id = 'world'
        marker.header.stamp = stamp
        marker.ns = name
        marker.id = marker_id
        marker.type = Marker.SPHERE
        marker.action = Marker.ADD

        marker.pose.position.x = float(position[0])
        marker.pose.position.y = float(position[1])
        marker.pose.position.z = float(position[2])
        marker.pose.orientation.w = 1.0

        marker.scale.x = size
        marker.scale.y = size
        marker.scale.z = size

        marker.color = color

        return marker

    def color(self, r, g, b, a):
        msg = ColorRGBA()
        msg.r = r
        msg.g = g
        msg.b = b
        msg.a = a
        return msg

    def make_transform(self, x, y, z, roll, pitch, yaw):
        cr = math.cos(roll)
        sr = math.sin(roll)
        cp = math.cos(pitch)
        sp = math.sin(pitch)
        cy = math.cos(yaw)
        sy = math.sin(yaw)

        r00 = cy * cp
        r01 = cy * sp * sr - sy * cr
        r02 = cy * sp * cr + sy * sr

        r10 = sy * cp
        r11 = sy * sp * sr + cy * cr
        r12 = sy * sp * cr - cy * sr

        r20 = -sp
        r21 = cp * sr
        r22 = cp * cr

        return [
            [r00, r01, r02, x],
            [r10, r11, r12, y],
            [r20, r21, r22, z],
            [0.0, 0.0, 0.0, 1.0]
        ]

    def matmul(self, a, b):
        result = [
            [0.0 for _ in range(4)]
            for _ in range(4)
        ]

        for i in range(4):
            for j in range(4):
                value = 0.0
                for k in range(4):
                    value += a[i][k] * b[k][j]
                result[i][j] = value

        return result

    def translation_from_matrix(self, matrix):
        return (
            matrix[0][3],
            matrix[1][3],
            matrix[2][3]
        )

    def rpy_to_quaternion(self, roll, pitch, yaw):
        cy = math.cos(yaw * 0.5)
        sy = math.sin(yaw * 0.5)
        cp = math.cos(pitch * 0.5)
        sp = math.sin(pitch * 0.5)
        cr = math.cos(roll * 0.5)
        sr = math.sin(roll * 0.5)

        qw = cr * cp * cy + sr * sp * sy
        qx = sr * cp * cy - cr * sp * sy
        qy = cr * sp * cy + sr * cp * sy
        qz = cr * cp * sy - sr * sp * cy

        return qx, qy, qz, qw


def main(args=None):
    rclpy.init(args=args)

    node = MoveItPlanningVisualizer()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

파일을 저장합니다.

Ctrl + O
Enter
Ctrl + X

---

11. 코드 구조 설명

moveit_planning_visualizer.py는 MoveIt 경로 계획 개념을 RViz2에서 확인하기 위한 ROS 2 노드입니다. 이 노드는 실제 MoveIt의 move_group을 실행하지는 않지만, MoveIt이 내부적으로 사용하는 주요 개념을 단순화하여 시각화합니다.

핵심 클래스는 다음입니다.

class MoveItPlanningVisualizer(Node):

이 클래스는 ROS 2 노드이며 다음 역할을 수행합니다.

로봇 팔 관절 상태 생성
전진기구학 계산
동차 변환 행렬 생성
로봇 팔 좌표계 TF 발행
현재 End-Effector 위치 계산
목표 Pose 계산
계획 경로 생성
RViz2 Marker 발행

초기화 함수는 다음 역할을 합니다.

def __init__(self):

초기화 함수 안에서 TF 발행기와 Marker 발행기를 생성합니다.

self.tf_broadcaster = TransformBroadcaster(self)
self.marker_pub = self.create_publisher(MarkerArray, '/moveit_demo/markers', 10)

각 객체의 의미는 다음과 같습니다.

TransformBroadcaster
→ 로봇 팔 좌표계 체인을 /tf 토픽으로 발행한다.

MarkerArray Publisher
→ 로봇 팔 링크, 관절, 목표점, 계획 경로를 /moveit_demo/markers 토픽으로 발행한다.

주기 실행은 다음 코드로 이루어집니다.

self.timer = self.create_timer(0.05, self.publish_demo)

이 코드는 0.05초마다 publish_demo() 함수를 실행합니다. 즉, 로봇 팔 상태와 경로 시각화가 계속 갱신됩니다.

---

12. 관절 상태와 MoveIt의 Current Robot State

MoveIt에서 로봇 팔의 현재 상태는 관절값으로 표현됩니다. 로봇 팔의 각 관절이 어느 각도로 회전해 있는지가 현재 로봇 상태를 결정합니다.

이번 실습에서는 시작 관절 상태와 목표 관절 상태를 다음과 같이 정의합니다.

start_joints = [0.2, 0.3, -0.4]
goal_joints = [1.0, -0.5, 0.6]

각 값의 의미는 다음과 같습니다.

start_joints
→ 로봇 팔의 시작 관절 상태

goal_joints
→ 로봇 팔의 목표 관절 상태

MoveIt 관점에서 보면 다음과 같이 해석할 수 있습니다.

start_joints
→ Current Robot State

goal_joints
→ Goal Robot State

코드에서는 execution_ratio 값을 이용하여 시작 관절 상태에서 목표 관절 상태로 부드럽게 이동하도록 만듭니다.

self.execution_ratio += 0.01 * self.execution_direction

execution_ratio는 0.0에서 1.0 사이의 값을 가집니다.

execution_ratio = 0.0
→ 시작 자세

execution_ratio = 1.0
→ 목표 자세

execution_ratio = 0.5
→ 시작 자세와 목표 자세의 중간 자세

관절값 보간은 다음 함수에서 수행됩니다.

def interpolate(self, start, goal, ratio):
    return start + (goal - start) * ratio

이 함수는 시작값과 목표값 사이의 중간값을 계산합니다.

ratio가 0이면 시작값
ratio가 1이면 목표값
ratio가 0.5이면 시작값과 목표값의 중간값

실제 MoveIt에서는 경로 계획 결과로 여러 개의 관절 상태가 포함된 Trajectory가 생성됩니다. 이번 실습에서는 이 개념을 단순화하여 시작 관절 상태와 목표 관절 상태 사이를 선형 보간으로 표현합니다.

---

13. 전진기구학과 End-Effector 위치 계산

전진기구학(Forward Kinematics)은 관절값을 알고 있을 때 로봇 팔 끝단의 위치와 방향을 계산하는 과정입니다. MoveIt에서도 로봇 팔의 현재 관절값을 이용하여 End-Effector의 현재 Pose를 계산합니다.

이번 실습에서 전진기구학은 다음 함수가 담당합니다.

def forward_kinematics(self, q1, q2, q3, link1, link2, link3):

입력값의 의미는 다음과 같습니다.

q1
→ shoulder 관절 회전값

q2
→ elbow 이전 링크 회전값

q3
→ wrist 이전 링크 회전값

link1
→ 첫 번째 링크 길이

link2
→ 두 번째 링크 길이

link3
→ 세 번째 링크 길이

함수 내부에서는 각 좌표계 사이의 변환을 동차 변환 행렬로 만듭니다.

t_world_base = self.make_transform(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
t_base_shoulder = self.make_transform(0.0, 0.0, 0.4, 0.0, 0.0, q1)
t_shoulder_upper = self.make_transform(link1, 0.0, 0.0, 0.0, q2, 0.0)
t_upper_elbow = self.make_transform(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
t_elbow_forearm = self.make_transform(link2, 0.0, 0.0, 0.0, q3, 0.0)
t_forearm_wrist = self.make_transform(link3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)

각 변환의 의미는 다음과 같습니다.

T_world_base
→ world 기준에서 base_link의 위치와 방향

T_base_shoulder
→ base_link 기준에서 shoulder_link의 위치와 방향

T_shoulder_upper
→ shoulder_link 기준에서 upper_arm_link의 위치와 방향

T_upper_elbow
→ upper_arm_link 기준에서 elbow_link의 위치와 방향

T_elbow_forearm
→ elbow_link 기준에서 forearm_link의 위치와 방향

T_forearm_wrist
→ forearm_link 기준에서 wrist_link의 위치와 방향

이 변환들은 순서대로 곱해집니다.

t_world_shoulder = self.matmul(t_world_base, t_base_shoulder)
t_world_upper = self.matmul(t_world_shoulder, t_shoulder_upper)
t_world_elbow = self.matmul(t_world_upper, t_upper_elbow)
t_world_forearm = self.matmul(t_world_elbow, t_elbow_forearm)
t_world_wrist = self.matmul(t_world_forearm, t_forearm_wrist)

이 계산은 다음 수식과 같습니다.

T_world_wrist =
T_world_base
T_base_shoulder
T_shoulder_upper
T_upper_elbow
T_elbow_forearm
T_forearm_wrist

즉, 로봇 팔 끝단 위치는 각 링크 사이의 변환을 순서대로 곱해서 계산됩니다.

💡 3편·6편과의 연결 — 3편에서 yaw를 quaternion으로 발행했고, 6편에서 FK 개념을 잡았습니다. 여기서는 그 둘이 합쳐져, 링크마다 동차 변환 행렬을 만들고 순서대로 곱해 tool0 위치를 구합니다.

---

14. 동차 변환 행렬 사용 위치

동차 변환 행렬은 회전과 이동을 하나의 행렬로 표현합니다. 일반적인 3차원 동차 변환 행렬은 다음 구조를 가집니다.

[ R  t ]
[ 0  1 ]

각 항목의 의미는 다음과 같습니다.

R
→ 3x3 회전 행렬

t
→ 3x1 이동 벡터

마지막 행
→ 동차 좌표 계산을 위한 행

이번 실습에서 동차 변환 행렬은 다음 함수에서 생성됩니다.

def make_transform(self, x, y, z, roll, pitch, yaw):

이 함수는 다음 정보를 입력받습니다.

x, y, z
→ 부모 좌표계에서 자식 좌표계까지의 이동

roll, pitch, yaw
→ 부모 좌표계에서 자식 좌표계까지의 회전

함수 내부에서는 Roll, Pitch, Yaw를 이용해 회전 행렬을 만듭니다.

r00 = cy * cp
r01 = cy * sp * sr - sy * cr
r02 = cy * sp * cr + sy * sr

r10 = sy * cp
r11 = sy * sp * sr + cy * cr
r12 = sy * sp * cr - cy * sr

r20 = -sp
r21 = cp * sr
r22 = cp * cr

최종적으로 다음과 같은 4x4 행렬을 반환합니다.

return [
    [r00, r01, r02, x],
    [r10, r11, r12, y],
    [r20, r21, r22, z],
    [0.0, 0.0, 0.0, 1.0]
]

이 행렬은 로봇 팔 링크 사이의 좌표계 변환을 의미합니다.

T_parent_child
→ child 좌표계의 점을 parent 좌표계로 변환하는 행렬

로봇 팔 끝단 위치는 다음과 같이 계산됩니다.

T_world_tool =
T_world_base
T_base_shoulder
T_shoulder_upper
T_upper_elbow
T_elbow_forearm
T_forearm_wrist
T_wrist_tool

MoveIt에서는 이러한 좌표계 체인을 기반으로 현재 End-Effector Pose를 계산하고, 목표 Pose까지 이동 가능한 경로를 계획합니다.

---

15. setup.py 수정

패키지 실행 명령을 등록합니다.

cd ~/moveit_planning_ws/src/moveit_planning_demo
nano setup.py

entry_points 부분을 다음과 같이 수정합니다.

entry_points={
    'console_scripts': [
        'moveit_planning_visualizer = moveit_planning_demo.moveit_planning_visualizer:main',
    ],
},

전체 setup.py의 핵심 구조는 다음과 같습니다.

from setuptools import find_packages, setup

package_name = 'moveit_planning_demo'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='robot',
    maintainer_email='robot@example.com',
    description='MoveIt planning concept visualization demo',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'moveit_planning_visualizer = moveit_planning_demo.moveit_planning_visualizer:main',
        ],
    },
)

파일을 저장합니다.

Ctrl + O
Enter
Ctrl + X

⚠️ entry_points 누락은 가장 흔한 함정 — 이게 없으면 ros2 run이 실행 파일을 못 찾습니다(오류 점검 5 참고).

---

16. package.xml 확인

package.xml 파일을 확인합니다.

cd ~/moveit_planning_ws/src/moveit_planning_demo
nano package.xml

다음 의존성이 포함되어 있는지 확인합니다.

<depend>rclpy</depend>
<depend>geometry_msgs</depend>
<depend>visualization_msgs</depend>
<depend>std_msgs</depend>
<depend>tf2_ros</depend>

파일을 확인한 뒤 닫습니다.

Ctrl + X

---

17. 패키지 빌드

작업공간 루트로 이동합니다.

cd ~/moveit_planning_ws

ROS 2 환경을 불러옵니다.

source /opt/ros/humble/setup.bash

패키지를 빌드합니다.

colcon build --packages-select moveit_planning_demo

정상적으로 빌드되면 다음과 유사한 출력이 나타납니다.

Starting >>> moveit_planning_demo
Finished <<< moveit_planning_demo

빌드 결과를 현재 터미널에 반영합니다.

source install/setup.bash

패키지가 인식되는지 확인합니다.

ros2 pkg list | grep moveit_planning_demo

예상 출력은 다음과 같습니다.

moveit_planning_demo

실행 파일이 등록되었는지 확인합니다.

ros2 pkg executables moveit_planning_demo

예상 출력은 다음과 같습니다.

moveit_planning_demo moveit_planning_visualizer

---

18. RViz2 실행 (터미널 1)

첫 번째 SSH 터미널에서 RViz2를 실행합니다. 기존 RViz2가 실행 중이면 먼저 종료합니다.

pkill -f "rviz2" || true

작업공간으로 이동합니다.

cd ~/moveit_planning_ws

ROS 2 환경과 작업공간 환경을 불러옵니다.

source /opt/ros/humble/setup.bash
source install/setup.bash

RViz2 실행 환경변수를 설정합니다.

export DISPLAY=:1
export LIBGL_ALWAYS_SOFTWARE=1

본 실습용 RViz2 설정 파일로 실행합니다.

rviz2 -d ~/moveit_planning_ws/rviz/moveit_planning_demo.rviz > ~/rviz2.log 2>&1 &

RViz2 프로세스를 확인합니다.

ps -ef | grep rviz2 | grep -v grep

RViz2 로그를 확인합니다.

tail -n 30 ~/rviz2.log

브라우저 noVNC 화면에서 RViz2 창이 열렸는지 확인합니다. 이 단계에서는 아직 로봇 팔이 보이지 않을 수 있습니다. Visualizer 노드가 아직 실행되지 않았기 때문입니다.

⚠️ LIBGL_ALWAYS_SOFTWARE=1 필수 — GPU 없는 VM에서 소프트웨어 렌더링을 강제합니다. 빠뜨리면 RViz2가 크래시할 수 있습니다(3편과 동일).

---

19. Visualizer 노드 실행 (터미널 2)

두 번째 SSH 터미널에서 Visualizer 노드를 실행합니다. 작업공간으로 이동합니다.

cd ~/moveit_planning_ws

ROS 2 환경과 작업공간 환경을 불러옵니다.

source /opt/ros/humble/setup.bash
source install/setup.bash

Visualizer 노드를 실행합니다.

ros2 run moveit_planning_demo moveit_planning_visualizer

정상 실행되면 다음과 유사한 로그가 출력됩니다.

[INFO] [moveit_planning_visualizer]: MoveIt planning concept visualizer started

이 터미널은 계속 실행 상태로 둡니다. 실행 중인 노드는 다음 데이터를 계속 발행합니다.

/tf
/moveit_demo/markers

---

20. 토픽 확인 (터미널 3)

세 번째 SSH 터미널에서 토픽을 확인합니다. 작업공간으로 이동합니다.

cd ~/moveit_planning_ws

ROS 2 환경과 작업공간 환경을 불러옵니다.

source /opt/ros/humble/setup.bash
source install/setup.bash

토픽 목록을 확인합니다.

ros2 topic list

예상 출력에는 다음 토픽이 포함되어야 합니다.

/tf
/moveit_demo/markers

MarkerArray 토픽 정보를 확인합니다.

ros2 topic info /moveit_demo/markers

예상 출력은 다음과 유사합니다.

Type: visualization_msgs/msg/MarkerArray
Publisher count: 1
Subscription count: 1

MarkerArray 메시지를 한 번 확인합니다.

ros2 topic echo /moveit_demo/markers --once

출력에서 다음 항목을 확인합니다.

markers:
- header:
    frame_id: world
  ns: robot_arm_links
- header:
    frame_id: world
  ns: robot_arm_joints
- header:
    frame_id: world
  ns: target_pose
- header:
    frame_id: world
  ns: planned_trajectory

frame_id: world는 Marker들이 RViz2의 Fixed Frame 기준으로 표시된다는 의미입니다.

---

21. TF 확인

이번 실습의 핵심은 로봇 팔의 링크와 관절이 하나의 좌표계 체인으로 연결되는 것을 확인하는 것입니다. TF 토픽을 한 번 확인합니다.

ros2 topic echo /tf --once

출력에는 다음과 같은 부모-자식 좌표계 관계가 포함되어야 합니다.

transforms:
- header:
    stamp:
      sec: 1781874064
      nanosec: 609330903
    frame_id: shoulder_link
  child_frame_id: upper_arm_link
  transform:
    translation:
      x: 0.75
      y: 0.0
      z: 0.0
    rotation:
      x: 0.0
      y: 0.09784320997617763
      z: 0.0
      w: 0.995201841970541

world에서 base_link까지의 변환을 확인합니다.

ros2 run tf2_ros tf2_echo world base_link

정상 출력은 다음과 유사합니다.

At time ...
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]

출력을 확인한 뒤 중지합니다.

Ctrl + C

world에서 tool0까지의 변환을 확인합니다.

ros2 run tf2_ros tf2_echo world tool0

정상적으로 실행되면 tool0의 위치와 방향이 계속 출력됩니다.

At time ...
- Translation: [x, y, z]
- Rotation: in Quaternion [x, y, z, w]

tool0는 로봇 팔 끝단 좌표계이므로, 관절값이 변하면 world 기준에서의 위치도 계속 변합니다. 출력을 확인한 뒤 중지합니다.

Ctrl + C

wrist_link에서 tool0까지의 변환을 확인합니다.

ros2 run tf2_ros tf2_echo wrist_link tool0

정상 출력은 다음과 유사합니다.

At time ...
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]

이번 실습 코드에서는 tool0를 wrist_link와 같은 위치에 두었습니다. 실제 산업용 로봇에서는 wrist_link와 tool0 사이에 그리퍼, 흡착기, 용접 토치, 카메라 등의 Tool Offset이 추가될 수 있습니다. 출력을 확인한 뒤 중지합니다.

Ctrl + C

---

22. RViz2 시각화 결과 확인

브라우저 noVNC 화면에서 RViz2를 확인합니다. RViz2 왼쪽 Displays 패널에는 다음 항목이 있어야 합니다.

Grid
TF
MoveIt Planning Markers

RViz2의 Fixed Frame은 다음으로 설정되어 있어야 합니다.

world

RViz2 화면에서 확인할 요소는 다음과 같습니다.

Grid
TF 좌표축
로봇 팔 링크 선
관절 구
현재 End-Effector 위치
목표 Pose
계획 경로
현재 End-Effector에서 목표 Pose까지의 연결선

RViz2에서 로봇 팔이 시작 자세와 목표 자세 사이를 반복적으로 움직이면 Visualizer 노드가 정상 실행 중인 것입니다.

💡 색깔로 구분 — 하늘색 선=링크, 주황색 구=관절, 빨간색 구=현재 tool0, 보라색 구=Target Pose, 노란색 선=Planned Trajectory, 옅은 빨간 선=현재 tool0↔목표 연결선.

 

---

23. Planning Frame 확인

MoveIt에서 Planning Frame은 경로 계획의 기준 좌표계입니다. 로봇 팔의 현재 상태, 목표 Pose, 장애물, 경로는 모두 Planning Frame을 기준으로 해석됩니다.

이번 실습에서 Planning Frame은 다음입니다.

world

RViz2에서도 Fixed Frame을 world로 설정했습니다.

Fixed Frame: world

즉, RViz2 화면에 표시되는 로봇 팔, 목표 Pose, 계획 경로는 모두 world 기준으로 표시됩니다. MoveIt 관점에서는 다음과 같이 해석합니다.

Planning Frame
→ world

Robot Base Frame
→ base_link

End-Effector Frame
→ tool0

world → base_link → ... → tool0 좌표계 체인이 정상적으로 연결되어야 MoveIt이 현재 End-Effector Pose를 계산할 수 있습니다.

---

24. End-Effector Pose 확인

MoveIt에서 End-Effector는 로봇 팔이 실제 작업을 수행하는 끝단입니다.

산업 현장에서는 그리퍼, 흡착기, 용접 토치, 카메라, 공구 등이 End-Effector가 됩니다.

이번 실습에서는 End-Effector Frame을 다음으로 설정했습니다.

tool0

tool0 위치를 TF로 확인합니다.

ros2 run tf2_ros tf2_echo world tool0

출력에서 Translation 값을 확인합니다.

- Translation: [x, y, z]

이 값은 world 기준에서 로봇 팔 끝단이 어디에 있는지를 의미합니다. RViz2에서는 빨간색 구가 현재 End-Effector 위치를 나타냅니다.

빨간색 구
→ 현재 tool0 위치

MoveIt에서 현재 End-Effector Pose는 다음 과정으로 계산됩니다.

현재 관절값
↓
전진기구학 계산
↓
world 기준 tool0 위치와 방향 계산

이번 실습에서는 이 과정을 코드 내부의 forward_kinematics() 함수와 RViz2 Marker로 확인합니다.

---

25. Target Pose 확인

MoveIt에서 Target Pose는 End-Effector가 도달해야 하는 목표 위치와 방향입니다.

이번 실습에서는 목표 Pose를 보라색 구로 표시합니다.

보라색 구
→ Target Pose

코드에서는 목표 관절 상태를 먼저 정의하고, 그 관절 상태에서의 tool0 위치를 목표 Pose로 사용합니다.

goal_joints = [1.0, -0.5, 0.6]

목표 관절 상태에서 계산된 tool0 위치는 다음 변수에 저장됩니다.

goal_tool = goal_points[-1]

즉, 이번 실습의 목표 Pose는 다음 의미를 가집니다.

goal_joints로 계산된 End-Effector 위치

실제 MoveIt에서는 사용자가 RViz2의 MotionPlanning 플러그인에서 Interactive Marker를 움직여 목표 Pose를 지정하거나, 프로그램 코드에서 PoseStamped 메시지로 목표 Pose를 지정합니다.

본 실습에서는 그 개념을 단순화하여 목표 위치를 보라색 Marker로 표시합니다.

---

26. Planned Trajectory 확인

MoveIt에서 Planned Trajectory는 현재 로봇 팔 상태에서 목표 Pose까지 이동하기 위해 계산된 경로입니다. 이번 실습에서는 계획 경로를 노란색 선으로 표시합니다.

노란색 선
→ Planned Trajectory

코드에서는 시작 End-Effector 위치와 목표 End-Effector 위치 사이를 보간하여 계획 경로를 만듭니다.

planned_path = self.generate_planned_path(start_tool, goal_tool)

계획 경로 생성 함수는 다음입니다.

def generate_planned_path(self, start_tool, goal_tool):

이 함수는 시작점과 목표점 사이에 여러 개의 중간점을 만듭니다.

start_tool
→ 시작 End-Effector 위치

goal_tool
→ 목표 End-Effector 위치

planned_path
→ 시작점에서 목표점까지의 경로 점 목록

경로 중간에 약간의 높이 변화를 주기 위해 다음 식을 사용합니다.

lift = 0.25 * math.sin(math.pi * ratio)

이 식은 경로가 단순 직선이 아니라 위로 살짝 들어 올려지는 곡선처럼 보이도록 만듭니다.

실제 MoveIt에서는 충돌 회피, 관절 제한, 속도 제한, 가속도 제한 등을 고려하여 경로를 생성합니다.

이번 실습에서는 MoveIt의 경로 계획 결과가 RViz2에서 어떤 방식으로 보이는지 이해하기 위해 단순화된 경로를 사용합니다.

---

27. Motion Execution 개념 확인

MoveIt에서 Motion Execution은 계획된 경로를 실제 로봇 팔 Controller에 전달하여 움직이게 하는 과정입니다.

이번 실습에서는 실제 Controller를 사용하지 않습니다.

대신 execution_ratio 값을 이용해 로봇 팔이 시작 자세와 목표 자세 사이를 반복적으로 움직이도록 합니다.

self.execution_ratio += 0.01 * self.execution_direction

execution_ratio의 의미는 다음과 같습니다.

0.0
→ 시작 자세

0.5
→ 경로 중간 자세

1.0
→ 목표 자세

코드에서는 이 값을 이용해 현재 관절값을 계산합니다.

q1 = self.interpolate(start_joints[0], goal_joints[0], self.execution_ratio)
q2 = self.interpolate(start_joints[1], goal_joints[1], self.execution_ratio)
q3 = self.interpolate(start_joints[2], goal_joints[2], self.execution_ratio)

실제 MoveIt에서는 다음 흐름으로 실행됩니다.

Planned Trajectory 생성
↓
Trajectory Controller로 전달
↓
각 관절 Controller가 목표 관절값을 따라감
↓
로봇 팔이 실제로 움직임

이번 실습에서는 이 흐름을 RViz2에서 시각적으로 모사합니다.

---

28. MoveIt 경로 계획 개념 해석

이번 실습의 RViz2 화면은 MoveIt 경로 계획 과정을 단순화하여 보여줍니다.

MoveIt의 경로 계획은 단순히 점과 점을 잇는 것이 아닙니다.

실제 MoveIt은 다음 조건을 함께 고려합니다.

로봇 팔의 관절 제한
End-Effector 목표 위치와 방향
장애물과의 충돌 가능성
로봇 자신의 링크 간 충돌 가능성
경로의 연속성
속도 제한
가속도 제한
Controller 실행 가능성

본 실습은 이 중 다음 개념을 RViz2에서 먼저 이해하도록 구성합니다.

좌표계 체인
End-Effector 위치
Target Pose
Trajectory
Motion Execution

---

29. 동차 변환 행렬 관점 정리

로봇 팔의 각 링크는 부모 좌표계와 자식 좌표계의 관계로 연결됩니다. 예를 들어 다음 관계가 있습니다.

base_link → shoulder_link
shoulder_link → upper_arm_link
upper_arm_link → elbow_link
elbow_link → forearm_link
forearm_link → wrist_link
wrist_link → tool0

각 관계는 회전과 이동을 가집니다.

이동
→ x, y, z

회전
→ roll, pitch, yaw

이동과 회전을 하나로 합치면 동차 변환 행렬이 됩니다.

T_parent_child =
[ R  t ]
[ 0  1 ]

로봇 팔 끝단 좌표계 tool0의 위치는 여러 변환 행렬을 순서대로 곱해서 계산합니다.

T_world_tool =
T_world_base
T_base_shoulder
T_shoulder_upper
T_upper_elbow
T_elbow_forearm
T_forearm_wrist
T_wrist_tool

어떤 점 p_tool이 tool0 기준으로 표현되어 있다면, world 기준 위치는 다음과 같이 계산됩니다.

p_world = T_world_tool p_tool

이번 실습에서 RViz2의 TF Display는 이 좌표계 체인을 시각적으로 보여줍니다.

즉, RViz2에 보이는 좌표축은 동차 변환 행렬 관계를 사람이 볼 수 있게 표시한 결과입니다. MoveIt은 이 좌표계 체인을 기반으로 다음을 계산합니다.

현재 End-Effector Pose
목표 Pose와의 차이
IK 해의 가능성
경로 계획 결과
Trajectory 실행 경로

---

30. 재실행 방법

본 실습을 다시 실행하려면 기존 노드와 RViz2를 먼저 중지합니다.

pkill -f "moveit_planning_visualizer" || true
pkill -f "rviz2" || true

ROS 2 데몬을 재시작합니다.

ros2 daemon stop
ros2 daemon start

첫 번째 SSH 터미널에서 RViz2를 다시 실행합니다.

cd ~/moveit_planning_ws
source /opt/ros/humble/setup.bash
source install/setup.bash

export DISPLAY=:1
export LIBGL_ALWAYS_SOFTWARE=1

rviz2 -d ~/moveit_planning_ws/rviz/moveit_planning_demo.rviz > ~/rviz2.log 2>&1 &

두 번째 SSH 터미널에서 Visualizer 노드를 다시 실행합니다.

cd ~/moveit_planning_ws
source /opt/ros/humble/setup.bash
source install/setup.bash

ros2 run moveit_planning_demo moveit_planning_visualizer

세 번째 SSH 터미널에서 토픽을 확인합니다.

ros2 topic list

예상 토픽은 다음과 같습니다.

/tf
/moveit_demo/markers

브라우저 noVNC 화면에서 RViz2 시각화 결과를 확인합니다.

---

31. 오류 점검 7종

실습 중 자주 겪는 오류를 증상 → 원인 → 해결로 정리합니다. 각 해결 명령은 그대로 따라 하면 됩니다.

오류 점검 1 — RViz2 창이 보이지 않는 경우

증상: RViz2를 실행했는데 브라우저 noVNC 화면에 창이 보이지 않음.
원인: DISPLAY 미설정, RViz2 프로세스 미실행, 화면 공유 스택 문제, 또는 창이 뒤에 가려짐.
해결: 먼저 DISPLAY 설정을 확인합니다.

echo $DISPLAY

정상 출력은 다음과 같습니다.

:1

값이 비어 있으면 다시 설정합니다.

export DISPLAY=:1

RViz2 프로세스를 확인합니다.

ps -ef | grep rviz2 | grep -v grep

RViz2가 실행 중이 아니면 다시 실행합니다.

cd ~/moveit_planning_ws
source /opt/ros/humble/setup.bash
source install/setup.bash

export DISPLAY=:1
export LIBGL_ALWAYS_SOFTWARE=1

rviz2 -d ~/moveit_planning_ws/rviz/moveit_planning_demo.rviz > ~/rviz2.log 2>&1 &

RViz2 로그를 확인합니다.

tail -n 30 ~/rviz2.log

화면 공유 프로세스가 정상인지 확인합니다.

ps -ef | grep -E "Xvfb|fluxbox|x11vnc|websockify|cloudflared" | grep -v grep

6080 포트를 확인합니다.

ss -lntp | grep 6080 || true

Cloudflared URL을 다시 확인합니다.

tail -n 20 ~/tunnel.log

브라우저 접속 주소는 다음 형식입니다.

https://xxxx-xxxx-xxxx.trycloudflare.com/vnc.html

브라우저 noVNC 화면이 정상이고 RViz2 프로세스도 실행 중이면, RViz2 창이 화면 뒤쪽에 가려져 있을 수 있습니다.

noVNC 화면 안에서 창 목록을 확인하거나, RViz2를 종료 후 다시 실행합니다.

오류 점검 2 — Marker가 보이지 않는 경우

증상: RViz2 창은 열렸지만 로봇 팔, 목표점, 계획 경로가 보이지 않음.
원인: Visualizer 노드 미실행(Publisher count 0), 또는 Fixed Frame이 world가 아님.
해결: /moveit_demo/markers 토픽을 확인합니다.

ros2 topic list

정상 출력에는 다음 토픽이 포함되어야 합니다.

/moveit_demo/markers

토픽 정보 확인:

ros2 topic info /moveit_demo/markers

정상 출력은 다음과 유사합니다.

Type: visualization_msgs/msg/MarkerArray
Publisher count: 1
Subscription count: 1

Publisher count가 0이면 Visualizer 노드가 실행 중이 아닙니다. 두 번째 SSH 터미널에서 다시 실행합니다.

cd ~/moveit_planning_ws
source /opt/ros/humble/setup.bash
source install/setup.bash

ros2 run moveit_planning_demo moveit_planning_visualizer

Marker 메시지를 한 번 확인합니다.

ros2 topic echo /moveit_demo/markers --once

출력에 다음 namespace가 포함되어야 합니다.

robot_arm_links
robot_arm_joints
start_tool_pose
current_tool_pose
target_pose
planned_trajectory
tool_to_target

RViz2 Display 설정도 확인합니다.

Display: MarkerArray
Topic: /moveit_demo/markers
Fixed Frame: world

Marker는 모두 frame_id: world 기준으로 발행됩니다.

따라서 RViz2의 Fixed Frame이 world가 아니면 정상 표시되지 않을 수 있습니다.

오류 점검 3 — TF가 보이지 않는 경우

증상: RViz2에서 좌표축이 보이지 않음.
원인: Visualizer 노드가 /tf를 발행하지 않음(노드 미실행).
해결: /tf 토픽을 확인합니다.

ros2 topic list

정상 출력에는 다음 토픽이 포함되어야 합니다.

/tf

TF 메시지를 확인합니다.

ros2 topic echo /tf --once

출력에 다음 좌표계 관계가 포함되어야 합니다.

world
base_link
shoulder_link
upper_arm_link
elbow_link
forearm_link
wrist_link
tool0

world에서 tool0까지 변환이 가능한지 확인합니다.

ros2 run tf2_ros tf2_echo world tool0

정상적으로 실행되면 Translation과 Rotation이 계속 출력됩니다.

At time ...
- Translation: [x, y, z]
- Rotation: in Quaternion [x, y, z, w]

출력을 중지합니다.

Ctrl + C

TF가 출력되지 않으면 Visualizer 노드가 실행 중인지 확인합니다.

ps -ef | grep moveit_planning_visualizer | grep -v grep

실행 중이 아니면 다시 실행합니다.

cd ~/moveit_planning_ws
source /opt/ros/humble/setup.bash
source install/setup.bash

ros2 run moveit_planning_demo moveit_planning_visualizer

오류 점검 4 — Fixed Frame 오류가 발생하는 경우

증상: RViz2 하단 또는 Displays 패널에서 다음과 같은 오류가 보임.

Fixed Frame [map] does not exist
No transform from [tool0] to [world]
Frame [world] does not exist

원인: Fixed Frame이 map(RViz 기본값) 등으로 잘못 설정됨.
해결: 이번 실습의 Fixed Frame은 반드시 다음으로 설정합니다.

world

RViz2 설정 파일에도 다음 항목이 있어야 합니다.

Global Options:
  Fixed Frame: world

설정 파일을 확인합니다.

grep -n "Fixed Frame" ~/moveit_planning_ws/rviz/moveit_planning_demo.rviz

정상 출력은 다음과 유사합니다.

Fixed Frame: world

RViz2를 설정 파일로 다시 실행합니다.

pkill -f "rviz2" || true

cd ~/moveit_planning_ws
source /opt/ros/humble/setup.bash
source install/setup.bash

export DISPLAY=:1
export LIBGL_ALWAYS_SOFTWARE=1

rviz2 -d ~/moveit_planning_ws/rviz/moveit_planning_demo.rviz > ~/rviz2.log 2>&1 &

Visualizer 노드가 실행 중인지 확인합니다.

ps -ef | grep moveit_planning_visualizer | grep -v grep

실행 중이 아니면 다시 실행합니다.

ros2 run moveit_planning_demo moveit_planning_visualizer

오류 점검 5 — ros2 run이 실패하는 경우

증상: 다음 명령이 실패함.

ros2 run moveit_planning_demo moveit_planning_visualizer

원인: 작업공간 환경 미적용, 패키지/실행 파일 미인식, entry_points 누락.
해결: 먼저 작업공간 환경을 불러왔는지 확인합니다.

cd ~/moveit_planning_ws
source /opt/ros/humble/setup.bash
source install/setup.bash

패키지가 인식되는지 확인합니다.

ros2 pkg list | grep moveit_planning_demo

실행 파일이 등록되었는지 확인합니다.

ros2 pkg executables moveit_planning_demo

정상 출력은 다음과 같아야 합니다.

moveit_planning_demo moveit_planning_visualizer

출력이 없다면 setup.py의 entry_points를 확인합니다.

cd ~/moveit_planning_ws/src/moveit_planning_demo
nano setup.py

다음 내용이 있어야 합니다.

entry_points={
    'console_scripts': [
        'moveit_planning_visualizer = moveit_planning_demo.moveit_planning_visualizer:main',
    ],
},

노드 파일이 존재하는지 확인합니다.

ls ~/moveit_planning_ws/src/moveit_planning_demo/moveit_planning_demo

다음 파일이 있어야 합니다.

moveit_planning_visualizer.py

다시 빌드합니다.

cd ~/moveit_planning_ws
colcon build --packages-select moveit_planning_demo
source install/setup.bash

다시 실행합니다.

ros2 run moveit_planning_demo moveit_planning_visualizer

오류 점검 6 — 코드 수정 후 반영되지 않는 경우

증상: Python 코드를 수정했는데 실행 결과가 바뀌지 않음.
원인: 빌드 후 작업공간 환경을 다시 source하지 않음, 또는 기존 노드가 살아 있음.
해결: 다시 빌드하고 작업공간 환경을 다시 불러옵니다.

cd ~/moveit_planning_ws
colcon build --packages-select moveit_planning_demo
source install/setup.bash

기존 노드를 종료합니다.

pkill -f "moveit_planning_visualizer" || true

다시 실행합니다.

ros2 run moveit_planning_demo moveit_planning_visualizer

여러 SSH 터미널을 사용하는 경우 각 터미널마다 다음 명령을 다시 실행해야 합니다.

cd ~/moveit_planning_ws
source /opt/ros/humble/setup.bash
source install/setup.bash

오류 점검 7 — 기존 실습 토픽이 섞이는 경우

증상: 기존 RViz 센서 실습 토픽(/demo/scan 등)이 계속 보임.
원인: 기존 노드가 종료되지 않고 남아 있음, ROS 2 데몬 캐시.
해결: 토픽 목록을 확인합니다.

ros2 topic list

본 실습에서 필요한 토픽은 다음입니다.

/tf
/moveit_demo/markers

다음 토픽이 보이면 이전 센서 실습이 섞인 것입니다.

/demo/scan
/demo/odom
/demo/path

기존 노드를 종료합니다.

pkill -f "sensor_demo_publisher" || true
pkill -f "arm_frame_visualizer" || true
pkill -f "moveit_planning_visualizer" || true
pkill -f "robot_state_publisher" || true
pkill -f "joint_state_publisher" || true
pkill -f "move_group" || true
pkill -f "static_transform_publisher" || true

ROS 2 데몬을 재시작합니다.

ros2 daemon stop
ros2 daemon start

본 실습 노드만 다시 실행합니다.

cd ~/moveit_planning_ws
source /opt/ros/humble/setup.bash
source install/setup.bash

ros2 run moveit_planning_demo moveit_planning_visualizer

다시 토픽 목록을 확인합니다.

ros2 topic list

정상 기준은 다음과 같습니다.

/tf
/moveit_demo/markers

---

32. 실습 종료

실습을 종료할 때 Visualizer 노드와 RViz2를 중지합니다.

pkill -f "moveit_planning_visualizer" || true
pkill -f "rviz2" || true

관련 프로세스가 남아 있는지 확인합니다.

ps -ef | grep -E "rviz2|moveit_planning_visualizer" | grep -v grep

아무 출력이 없으면 본 실습 관련 프로세스가 종료된 것입니다. 화면 공유 프로세스까지 종료할 필요가 있으면 다음 명령을 사용합니다.

pkill -f "Xvfb :1" || true
pkill -f "fluxbox" || true
pkill -f "x11vnc" || true
pkill -f "websockify" || true
pkill -f "cloudflared tunnel" || true
pkill -f "xterm" || true

단, 이후 다른 RViz2 실습을 이어서 진행할 예정이면 화면 공유 프로세스는 유지합니다.

---

33. 실습 결과 정리

이번 실습에서는 실제 MoveIt 전체 시스템을 구성하지 않고, MoveIt 경로 계획의 핵심 개념을 ROS 2와 RViz2로 시각화했습니다.

생성한 작업공간은 다음과 같습니다.

~/moveit_planning_ws

생성한 패키지는 다음과 같습니다.

moveit_planning_demo

작성한 노드는 다음과 같습니다.

moveit_planning_visualizer

발행한 토픽은 다음과 같습니다.

/tf
/moveit_demo/markers

RViz2에서 사용한 Display는 다음과 같습니다.

Grid
TF
MarkerArray

RViz2의 Fixed Frame은 다음과 같습니다.

world

확인한 좌표계 체인은 다음과 같습니다.

world
└── base_link
    └── shoulder_link
        └── upper_arm_link
            └── elbow_link
                └── forearm_link
                    └── wrist_link
                        └── tool0

확인한 시각화 요소는 다음과 같습니다.

로봇 팔 링크
로봇 팔 관절
현재 End-Effector 위치
Target Pose
Planned Trajectory
tool0와 Target Pose 사이의 거리
TF 좌표축

---

34. 실습 핵심 정리

MoveIt 경로 계획은 다음 흐름으로 이해할 수 있습니다.

현재 관절 상태 확인
↓
전진기구학으로 End-Effector Pose 계산
↓
Target Pose 지정
↓
경로 계획
↓
Trajectory 생성
↓
Controller로 실행

이번 실습에서는 이 흐름을 RViz2에서 다음과 같이 확인했습니다.

현재 관절 상태
→ 움직이는 로봇 팔 자세

전진기구학
→ tool0 위치 계산

End-Effector Pose
→ 빨간색 Marker

Target Pose
→ 보라색 Marker

Planned Trajectory
→ 노란색 선

Motion Execution
→ 시작 자세와 목표 자세 사이를 반복 이동하는 로봇 팔

동차 변환 행렬 관점에서는 다음이 핵심입니다.

각 링크 사이의 위치와 방향은 하나의 변환 행렬로 표현된다.
각 변환 행렬은 부모 좌표계에서 자식 좌표계로 이어지는 관계이다.
로봇 팔 끝단 위치는 여러 변환 행렬을 순서대로 곱해 계산된다.
RViz2의 TF Display는 이 변환 행렬 체인을 좌표축으로 시각화한다.

수식으로 정리하면 다음과 같습니다.

T_world_tool =
T_world_base
T_base_shoulder
T_shoulder_upper
T_upper_elbow
T_elbow_forearm
T_forearm_wrist
T_wrist_tool

tool0 기준의 점을 world 기준으로 변환하면 다음과 같습니다.

p_world = T_world_tool p_tool

MoveIt은 이 좌표계 체인을 기반으로 현재 Pose, 목표 Pose, 경로 계획, 실행 경로를 계산합니다.

이번 실습은 그 구조를 RViz2에서 직접 확인하는 데 목적이 있습니다.

---

35. 실제 MoveIt 구성과 본 실습의 차이

이번 실습은 MoveIt의 핵심 개념을 이해하기 위한 시각화 실습입니다.

실제 MoveIt 전체 구성을 실행한 것은 아닙니다.

실제 MoveIt 환경에서는 다음 구성요소가 함께 사용됩니다.

URDF
SRDF
robot_state_publisher
joint_state_publisher
move_group
Planning Scene
Planning Pipeline
Kinematics Plugin
Collision Checker
Trajectory Controller
RViz2 MotionPlanning Plugin

이번 실습에서는 위 구성 중 핵심 개념을 단순화하여 표현했습니다. 따라서 이번 실습은 실제 로봇 팔 제어 실습이 아니라, MoveIt 경로 계획과 제어를 이해하기 위한 개념 시각화 실습입니다.

---

36. 실제 MoveIt 실습으로 확장할 때 필요한 구성

이번 실습 이후 실제 MoveIt 기반 로봇 팔 경로 계획 실습으로 확장하려면 다음 구성이 필요합니다.

로봇 URDF 작성
관절 구조 정의
링크 구조 정의
관성, 충돌, 시각 모델 정의
SRDF 작성
Planning Group 정의
End-Effector 정의
Kinematics Solver 설정
Joint Limit 설정
MoveIt Config Package 생성
move_group 실행
RViz2 MotionPlanning Plugin 실행
목표 Pose 지정
Plan 실행
Execute 실행
Controller 연동

실제 MoveIt 실습 흐름은 다음과 같습니다.

로봇 모델 준비
↓
MoveIt 설정 패키지 생성
↓
move_group 실행
↓
RViz2 MotionPlanning Plugin 실행
↓
Planning Group 선택
↓
End-Effector Goal 지정
↓
Plan
↓
Execute
↓
Trajectory Controller 실행

이번 실습은 이 전체 흐름 중 다음 부분을 먼저 이해하기 위한 단계입니다.

좌표계 체인
End-Effector Pose
Target Pose
Trajectory
Motion Execution 개념
동차 변환 행렬

---

37. 본 실습에서 확인해야 할 최종 화면

RViz2 화면에서 다음 요소가 확인되어야 합니다.

Grid가 표시된다.
TF 좌표축이 표시된다.
world 좌표계가 기준으로 설정된다.
base_link가 world에 연결된다.
shoulder_link, upper_arm_link, elbow_link, forearm_link, wrist_link, tool0가 체인으로 연결된다.
로봇 팔 링크가 선으로 표시된다.
관절 위치가 구로 표시된다.
현재 tool0 위치가 빨간색으로 표시된다.
목표 Pose가 보라색으로 표시된다.
계획 경로가 노란색 선으로 표시된다.
로봇 팔이 시작 자세와 목표 자세 사이를 반복 이동한다.

이 화면이 보이면 다음 개념을 확인한 것입니다.

로봇 팔 좌표계 구조
전진기구학 결과
End-Effector Pose
Target Pose
Planned Trajectory
Motion Execution 개념
동차 변환 행렬 체인

---

7편 정리

• 실제 move_group 없이 MoveIt 경로 계획의 핵심 개념을 ROS 2 TF + RViz2 Marker로 시각화했습니다.
• 본 주제 전용 작업공간 ~/moveit_planning_ws + 패키지 moveit_planning_demo + 노드 moveit_planning_visualizer를 만들었습니다.
• 노드는 관절 보간 → 전진기구학(동차 변환 행렬 곱) → tool0 위치 계산 → TF/MarkerArray 발행을 0.05초마다 반복합니다.
• RViz2에서 좌표계 체인(world → ... → tool0), 현재 End-Effector(빨간 구), Target Pose(보라 구), Planned Trajectory(노란 선)를 확인하고, 로봇 팔이 시작↔목표 자세를 반복 이동하는 것을 봤습니다.
• 오류 점검 7종(RViz 창 / Marker / TF / Fixed Frame / ros2 run / 코드 미반영 / 토픽 섞임)을 증상→원인→해결로 정리했습니다.

이로써 W2D2 운동학·MoveIt 시리즈(총 7편) 가 마무리됩니다. 다음 단계는 URDF/SRDF로 실제 로봇 모델을 정의하고 move_group을 띄우는 본격 MoveIt 실습입니다(36절의 확장 구성 참고).

---

시리즈 목차

1. [Physical AI W2D2] 1/7 — 운동학 개념 ①: 정운동학(링크·관절·자유도·2축 팔·동차변환행렬)
2. [Physical AI W2D2] 2/7 — 운동학 개념 ②: 역운동학·작업공간·자코비안·특이점
3. [Physical AI W2D2] 3/7 — 정운동학·역운동학 Python 실습(직접 계산·검증·관절제한·특이점)
4. [Physical AI W2D2] 4/7 — 계산 결과를 RViz2로 시각화(FK/IK → 화면)
5. [Physical AI W2D2] 5/7 — MoveIt 개념 ①: 경로계획 핵심(Planning Group·Scene·Collision·Trajectory)
6. [Physical AI W2D2] 6/7 — MoveIt 개념 ②: 계획 기법·Move Group·ROS 2 Control
7. [Physical AI W2D2] 7/7 — MoveIt 경로계획 RViz2 시각화 실습(+ 오류 점검 7종) — 이번 글