[Physical AI W2D2] 4/7 — 계산 결과를 RViz2로 시각화(FK/IK → 화면)

[Physical AI W2D2 · 4/7]

3편에서 Python으로 계산한 2축 로봇 팔의 정운동학·역운동학 결과를, 이번엔 ROS2 노드로 다시 계산해 RViz2 화면에 띄운다.
/tf로 world→base_link→link1→link2→tool0 좌표계를, MarkerArray로 링크·관절·목표점·작업공간을 그려, Python 숫자 결과와 화면 위치가 정확히 일치함을 눈으로 검증한다.

이 글에서 직접 해보는 것
1. ROS2 작업공간·패키지(two_link_kinematics_demo) 생성
2. .rviz 설정 파일로 Fixed Frame world + Grid·TF·MarkerArray Display 고정
3. two_link_kinematics_visualizer.py — FK/IK를 계산해 /tf + /kinematics_demo/markers 발행
4. GCP VM 헤드리스 GUI 스택(Xvfb→x11vnc→websockify→Cloudflared)으로 브라우저에서 RViz2 확인
5. 5개 예제(FK 2개·IK 2개·도달 불가 1개)에서 Python 숫자 ↔ RViz 화면 위치 일치 검증

(3편에서 Python으로 정운동학·역운동학을 계산하고 검증했다면, 이제 그 똑같은 계산을 ROS2 노드로 옮겨 RViz2 화면에 띄웁니다. 숫자로만 봤던 tool0 위치가 화면 위 점으로 찍히는 순간입니다.)

---

들어가며 — 숫자를 화면 위 점으로

3편까지 우리는 2축 평면 로봇 팔의 정운동학(FK) 과 역운동학(IK) 을 Python(math 표준 라이브러리)으로 계산했습니다. theta1=30도, theta2=45도를 넣으면 tool0 = (1.1248, 1.4659)라는 숫자가 나왔죠.

하지만 숫자만 봐서는 그 팔이 실제로 어떤 모양인지, 목표점에 닿는지 직관적으로 와닿지 않습니다.

이번 실습은 같은 계산을 ROS2 노드로 다시 수행하고, 그 결과를 RViz2 화면에 그립니다.

• /tf → world → base_link → link1 → link2 → tool0 좌표계를 표시
• /kinematics_demo/markers(MarkerArray) → 로봇 팔 링크, 관절, 목표점, 작업공간을 표시

그러면 Python이 내놓은 tool0 좌표와 RViz2 화면 위 빨간 점의 위치가 정확히 같다는 걸 눈으로 확인할 수 있습니다.

💡 RViz는 GUI라 화면이 필요합니다. GCP VM엔 모니터가 없으므로 W2D1 3편에서 세운 헤드리스 GUI 스택(Xvfb→x11vnc→websockify→Cloudflared)을 그대로 씁니다.
그 스택의 원리(왜 가상 화면이 필요한지, 포트가 어떻게 연결되는지)는 W2D1 3편에서 자세히 다뤘으니,
여기서는 이 실습이 주는 명령을 순서대로 실행하는 데 집중합니다.

 

💡 터미널 역할 분리 — ROS 2는 여러 노드가 동시에 돌므로 SSH 터미널을 나눕니다.
터미널 1: 빌드·화면 공유 스택·RViz2,
터미널 2: 시각화 노드 실행,
터미널 3: 토픽 확인.

---

1. 실습 개요 — 무엇을 / 왜 시각화하나

본 실습은 ROS2와 RViz2가 이미 설치된 GCP VM 환경(W2D1 3편에서 구축)을 기준으로 진행합니다.

앞서 Python으로 계산한 2축 평면 로봇 팔의 정운동학·역운동학 결과를 ROS2 노드로 다시 계산하고, RViz2에서 로봇 팔 자세와 목표 위치로 시각화합니다.

 

Python 실습에서 사용한 정운동학 공식:

x = L1 cos(θ1) + L2 cos(θ1 + θ2)
y = L1 sin(θ1) + L2 sin(θ1 + θ2)

Python 실습에서 사용한 역운동학 공식:

cos(θ2) = (x² + y² - L1² - L2²) / (2L1L2)

θ1 = atan2(y, x) - atan2(L2 sin(θ2), L1 + L2 cos(θ2))

이번 RViz2 실습에서는 다음 값을 직접 시각화합니다.

base = (0, 0)

joint1 = (
    L1 cos(θ1),
    L1 sin(θ1)
)

tool0 = (
    L1 cos(θ1) + L2 cos(θ1 + θ2),
    L1 sin(θ1) + L2 sin(θ1 + θ2)
)

RViz2에서 tool0 위치는 Python 정운동학 계산 결과의 x, y와 같습니다. 실습 화면은 Cloudflared 외부 접속으로 확인하며, 기준은 /tf, MarkerArray, world Fixed Frame 구조입니다.

💡 base는 팔의 뿌리(원점), joint1은 첫 번째 관절(팔꿈치), tool0은 말단 장치(손끝)입니다.
두 직선 링크(base→joint1, joint1→tool0)가 이어진 단순한 2축 팔을 그립니다.

---

2. 실습 실행 구조 — 화면은 어떻게 브라우저까지 오나

GCP VM 안에서 RViz2가 그린 화면이 브라우저까지 전달되는 경로입니다(W2D1 3편의 스택).

GCP VM
↓
Xvfb
↓
fluxbox
↓
x11vnc
↓
websockify
↓
Cloudflared
↓
브라우저 noVNC 화면
↓
RViz2

그리고 ROS2 시각화 구조(노드가 발행하는 두 가지)는 다음과 같습니다.

/tf
→ world, base_link, link1, link2, tool0 좌표계 표시

/kinematics_demo/markers
→ 로봇 팔 링크, 관절, 목표점, 작업공간 표시

좌표계 구조(부모→자식 트리):

world
└── base_link
    └── link1
        └── link2
            └── tool0

💡 /tf는 좌표계(축)를, /kinematics_demo/markers는 그 위에 그려지는 그림(선·구·원)을 담당합니다.
두 토픽이 합쳐져 "팔처럼 보이는" 화면이 됩니다.

---

3. 실습 예제 기준 — 5개 케이스

본 실습은 Python 계산 실습과 같은 예제를 사용합니다. 노드가 이 5개 케이스를 순서대로 화면에 보여줍니다.

정운동학 예제 1
theta1 = 30도
theta2 = 45도
예상 tool0 위치: x = 1.1248, y = 1.4659

정운동학 예제 2
theta1 = 90도
theta2 = -90도
예상 tool0 위치: x = 1.0000, y = 1.0000

역운동학 예제 1
target_x = 1.0
target_y = 1.0
Solution A: theta1 = 0도, theta2 = 90도

역운동학 예제 2
target_x = 1.0
target_y = 1.0
Solution B: theta1 = 90도, theta2 = -90도

도달 불가능 예제
target_x = 3.0
target_y = 0.0
결과: no solution

💡 IK 예제 1·2는 같은 목표점 (1.0, 1.0) 인데 자세가 둘(Solution A/B)입니다.
3편에서 본 "팔꿈치 위/아래" 두 해가 그대로 화면에 나타납니다.
도달 불가능 예제는 작업공간(반지름 L1+L2=2.0) 밖이라 목표점이 빨간색으로 표시됩니다.

---

4. 기존 프로세스와 포트 정리 — 깨끗한 상태에서 시작

이전 실습의 프로세스나 점유된 포트가 남아 있으면 RViz·화면 공유가 충돌합니다.

무엇을: 관련 프로세스·포트·로그·X 잠금 파일을 정리하고 ROS 데몬을 재시작합니다.

왜: 5900(VNC)·6080(웹) 포트와 :1 디스플레이를 비워야 새 스택이 깨끗이 뜹니다.

pkill -f "rviz2" || true
pkill -f "two_link_kinematics_visualizer" || true
pkill -f "moveit_planning_visualizer" || true
pkill -f "robot_state_publisher" || true
pkill -f "joint_state_publisher" || true
pkill -f "static_transform_publisher" || true
pkill -f "Xvfb :1" || true
pkill -f "fluxbox" || true
pkill -f "x11vnc" || true
pkill -f "websockify" || true
pkill -f "cloudflared" || true
fuser -k 5900/tcp || true
fuser -k 6080/tcp || true
rm -f ~/xvfb.log ~/fluxbox.log ~/x11vnc.log ~/websockify.log ~/tunnel.log ~/rviz2.log
rm -rf /tmp/.X1-lock /tmp/.X11-unix/X1
source /opt/ros/humble/setup.bash
ros2 daemon stop
ros2 daemon start

💡 || true는 "죽일 프로세스가 없어도 에러로 멈추지 말라"는 뜻입니다.
처음 실행이라 아무것도 안 떠 있어도 그냥 통과합니다.
기대 결과: 에러 없이 명령들이 지나가고 ros2 daemon이 재시작됩니다.

---

5. 작업공간 생성

무엇을: 이번 실습 전용 ROS2 작업공간을 새로 만듭니다.

왜: 패키지를 빌드(colcon build)하려면 src/가 있는 작업공간이 필요합니다.

rm -rf ~/ros2_kinematics_ws
mkdir -p ~/ros2_kinematics_ws/src
cd ~/ros2_kinematics_ws/src
source /opt/ros/humble/setup.bash

⚠️ rm -rf ~/ros2_kinematics_ws는 기존 작업공간을 통째로 지웁니다.
같은 이름의 중요한 작업이 있다면 먼저 백업하세요.
기대 결과: 빈 ~/ros2_kinematics_ws/src 디렉터리가 생기고, 현재 위치가 그 안으로 이동합니다.

---

6. ROS2 패키지 생성

무엇을: 시각화 노드를 담을 Python 패키지를 만듭니다.

왜: --dependencies로 미리 의존성(메시지 타입·TF)을 선언해 package.xml에 자동 등록되게 합니다.

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

의존성	역할
rclpy	ROS2 Python 클라이언트(노드 작성)
geometry_msgs	Point, TransformStamped(좌표·변환)
visualization_msgs	Marker, MarkerArray(RViz 그림)
std_msgs	ColorRGBA(마커 색)
tf2_ros	TransformBroadcaster(/tf 발행)

기대 결과: ~/ros2_kinematics_ws/src/two_link_kinematics_demo/ 패키지 골격이 생성됩니다.

---

7. RViz2 설정 파일 작성

무엇을: RViz2가 띄울 화면 구성을 .rviz 파일로 고정합니다.

왜: 매번 Display를 손으로 추가하면 사람마다 화면이 달라집니다.

설정 파일로 Fixed Frame world + Grid·TF·MarkerArray를 못 박습니다.

mkdir -p ~/ros2_kinematics_ws/rviz
nano ~/ros2_kinematics_ws/rviz/two_link_kinematics.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.4
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Update Interval: 0
      Value: true

    - Class: rviz_default_plugins/MarkerArray
      Enabled: true
      Name: Kinematics Markers
      Topic:
        Value: /kinematics_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: 5
      Focal Point:
        X: 0.8
        Y: 0.6
        Z: 0
      Name: Current View
      Pitch: 1.35
      Target Frame: <Fixed Frame>
      Yaw: 0.8

저장:

Ctrl + O
Enter
Ctrl + X

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

⚠️ Fixed Frame 함정 — 이 설정은 Fixed Frame: world로 못 박혀 있습니다.
만약 RViz가 기본값(map)으로 떨어지면 Global Status: Error — Frame [map] does not exist 가 뜹니다. 우리 노드는 world → base_link → ...만 발행하므로, Fixed Frame은 반드시 world여야 합니다.
위 전체본을 손으로 줄이거나 요약하면 RViz가 일부만 읽고 기본값으로 떨어질 수 있으니 전체를 그대로 넣으세요.
기대 결과: two_link_kinematics.rviz 파일이 저장됩니다.

---

8. 시각화 노드 작성

무엇을: FK/IK를 계산해 /tf와 /kinematics_demo/markers를 발행하는 노드를 작성합니다. 왜: 이 노드가 3편의 Python 계산을 ROS2 안에서 똑같이 수행하고, 결과를 RViz가 알아듣는 메시지로 바꿔 발행합니다.

cd ~/ros2_kinematics_ws/src/two_link_kinematics_demo/two_link_kinematics_demo
nano two_link_kinematics_visualizer.py

아래 코드를 한 글자도 바꾸지 말고(들여쓰기 포함) 그대로 넣습니다.

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 TwoLinkKinematicsVisualizer(Node):
    def __init__(self):
        super().__init__('two_link_kinematics_visualizer')

        self.l1 = 1.0
        self.l2 = 1.0

        self.demo_cases = [
            {
                'name': 'FK theta1=30 theta2=45',
                'mode': 'fk',
                'theta1_deg': 30.0,
                'theta2_deg': 45.0,
                'target_x': None,
                'target_y': None,
                'solution_index': None
            },
            {
                'name': 'FK theta1=90 theta2=-90',
                'mode': 'fk',
                'theta1_deg': 90.0,
                'theta2_deg': -90.0,
                'target_x': None,
                'target_y': None,
                'solution_index': None
            },
            {
                'name': 'IK target=(1.0, 1.0) solution A',
                'mode': 'ik',
                'theta1_deg': None,
                'theta2_deg': None,
                'target_x': 1.0,
                'target_y': 1.0,
                'solution_index': 0
            },
            {
                'name': 'IK target=(1.0, 1.0) solution B',
                'mode': 'ik',
                'theta1_deg': None,
                'theta2_deg': None,
                'target_x': 1.0,
                'target_y': 1.0,
                'solution_index': 1
            },
            {
                'name': 'IK target=(3.0, 0.0) no solution',
                'mode': 'ik',
                'theta1_deg': None,
                'theta2_deg': None,
                'target_x': 3.0,
                'target_y': 0.0,
                'solution_index': 0
            }
        ]

        self.case_index = 0
        self.tick = 0

        self.tf_broadcaster = TransformBroadcaster(self)

        self.marker_pub = self.create_publisher(
            MarkerArray,
            '/kinematics_demo/markers',
            10
        )

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

        self.get_logger().info('Two-link kinematics visualizer started')

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

        self.tick += 1

        if self.tick % 240 == 0:
            self.case_index = (self.case_index + 1) % len(self.demo_cases)

        case = self.demo_cases[self.case_index]

        theta1 = 0.0
        theta2 = 0.0
        reachable = True

        target_x = None
        target_y = None

        if case['mode'] == 'fk':
            theta1 = math.radians(case['theta1_deg'])
            theta2 = math.radians(case['theta2_deg'])

            base, joint1, tool = self.forward_kinematics(theta1, theta2)

            target_x = tool[0]
            target_y = tool[1]

        elif case['mode'] == 'ik':
            target_x = case['target_x']
            target_y = case['target_y']

            solutions = self.inverse_kinematics(target_x, target_y)

            if solutions is None:
                reachable = False
                theta1 = 0.0
                theta2 = 0.0
            else:
                theta1, theta2 = solutions[case['solution_index']]
                reachable = True

            base, joint1, tool = self.forward_kinematics(theta1, theta2)

        self.publish_tf(stamp, theta1, theta2)

        self.publish_markers(
            stamp,
            base,
            joint1,
            tool,
            target_x,
            target_y,
            reachable
        )

        if self.tick % 80 == 0:
            self.print_case_result(
                case,
                theta1,
                theta2,
                tool,
                target_x,
                target_y,
                reachable
            )

    def forward_kinematics(self, theta1, theta2):
        base = (0.0, 0.0, 0.0)

        joint1_x = self.l1 * math.cos(theta1)
        joint1_y = self.l1 * math.sin(theta1)

        joint1 = (
            joint1_x,
            joint1_y,
            0.0
        )

        tool_x = self.l1 * math.cos(theta1) + self.l2 * math.cos(theta1 + theta2)
        tool_y = self.l1 * math.sin(theta1) + self.l2 * math.sin(theta1 + theta2)

        tool = (
            tool_x,
            tool_y,
            0.0
        )

        return base, joint1, tool

    def inverse_kinematics(self, x, y):
        r2 = x ** 2 + y ** 2

        cos_theta2 = (
            r2 - self.l1 ** 2 - self.l2 ** 2
        ) / (
            2.0 * self.l1 * self.l2
        )

        if cos_theta2 < -1.0 or cos_theta2 > 1.0:
            return None

        theta2_a = math.acos(cos_theta2)
        theta2_b = -math.acos(cos_theta2)

        theta1_a = math.atan2(y, x) - math.atan2(
            self.l2 * math.sin(theta2_a),
            self.l1 + self.l2 * math.cos(theta2_a)
        )

        theta1_b = math.atan2(y, x) - math.atan2(
            self.l2 * math.sin(theta2_b),
            self.l1 + self.l2 * math.cos(theta2_b)
        )

        return (theta1_a, theta2_a), (theta1_b, theta2_b)

    def publish_tf(self, stamp, theta1, theta2):
        self.tf_broadcaster.sendTransform(
            self.make_tf(
                stamp,
                'world',
                'base_link',
                0.0,
                0.0,
                0.0,
                0.0
            )
        )

        self.tf_broadcaster.sendTransform(
            self.make_tf(
                stamp,
                'base_link',
                'link1',
                self.l1,
                0.0,
                0.0,
                theta1
            )
        )

        self.tf_broadcaster.sendTransform(
            self.make_tf(
                stamp,
                'link1',
                'link2',
                self.l2,
                0.0,
                0.0,
                theta2
            )
        )

        self.tf_broadcaster.sendTransform(
            self.make_tf(
                stamp,
                'link2',
                'tool0',
                0.0,
                0.0,
                0.0,
                0.0
            )
        )

    def publish_markers(
        self,
        stamp,
        base,
        joint1,
        tool,
        target_x,
        target_y,
        reachable
    ):
        marker_array = MarkerArray()

        target = (
            target_x,
            target_y,
            0.0
        )

        marker_array.markers.append(
            self.make_line_marker(
                stamp,
                0,
                'robot_arm',
                [base, joint1, tool],
                0.06,
                self.color(0.1, 0.8, 1.0, 1.0)
            )
        )

        marker_array.markers.append(
            self.make_sphere_marker(
                stamp,
                1,
                'base_joint',
                base,
                0.13,
                self.color(1.0, 0.6, 0.1, 1.0)
            )
        )

        marker_array.markers.append(
            self.make_sphere_marker(
                stamp,
                2,
                'elbow_joint',
                joint1,
                0.13,
                self.color(1.0, 0.6, 0.1, 1.0)
            )
        )

        marker_array.markers.append(
            self.make_sphere_marker(
                stamp,
                3,
                'tool0_current_position',
                tool,
                0.14,
                self.color(1.0, 0.1, 0.1, 1.0)
            )
        )

        if reachable:
            target_color = self.color(0.2, 1.0, 0.2, 1.0)
        else:
            target_color = self.color(1.0, 0.0, 0.0, 1.0)

        marker_array.markers.append(
            self.make_sphere_marker(
                stamp,
                4,
                'target_position',
                target,
                0.16,
                target_color
            )
        )

        marker_array.markers.append(
            self.make_line_marker(
                stamp,
                5,
                'tool_to_target_error',
                [tool, target],
                0.025,
                self.color(1.0, 1.0, 0.1, 0.8)
            )
        )

        marker_array.markers.append(
            self.make_line_marker(
                stamp,
                6,
                'workspace_boundary',
                self.make_workspace_circle_points(self.l1 + self.l2),
                0.025,
                self.color(0.4, 0.4, 1.0, 0.8)
            )
        )

        self.marker_pub.publish(marker_array)

    def make_workspace_circle_points(self, radius):
        points = []

        for i in range(121):
            angle = 2.0 * math.pi * i / 120.0

            x = radius * math.cos(angle)
            y = radius * math.sin(angle)

            points.append((x, y, 0.0))

        return points

    def make_tf(self, stamp, parent, child, x, y, z, yaw):
        msg = TransformStamped()

        msg.header.stamp = stamp
        msg.header.frame_id = parent
        msg.child_frame_id = child

        msg.transform.translation.x = float(x)
        msg.transform.translation.y = float(y)
        msg.transform.translation.z = float(z)

        qx, qy, qz, qw = self.yaw_to_quaternion(yaw)

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

        return msg

    def make_line_marker(
        self,
        stamp,
        marker_id,
        namespace,
        points,
        width,
        color
    ):
        marker = Marker()

        marker.header.frame_id = 'world'
        marker.header.stamp = stamp

        marker.ns = namespace
        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,
        namespace,
        position,
        size,
        color
    ):
        marker = Marker()

        marker.header.frame_id = 'world'
        marker.header.stamp = stamp

        marker.ns = namespace
        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 = float(r)
        msg.g = float(g)
        msg.b = float(b)
        msg.a = float(a)

        return msg

    def yaw_to_quaternion(self, yaw):
        qx = 0.0
        qy = 0.0
        qz = math.sin(yaw * 0.5)
        qw = math.cos(yaw * 0.5)

        return qx, qy, qz, qw

    def print_case_result(
        self,
        case,
        theta1,
        theta2,
        tool,
        target_x,
        target_y,
        reachable
    ):
        if reachable:
            error = math.sqrt(
                (target_x - tool[0]) ** 2 +
                (target_y - tool[1]) ** 2
            )
        else:
            error = None

        if error is None:
            self.get_logger().info(
                f"{case['name']} | "
                f"reachable=False"
            )
        else:
            self.get_logger().info(
                f"{case['name']} | "
                f"theta1={math.degrees(theta1):.2f} deg, "
                f"theta2={math.degrees(theta2):.2f} deg, "
                f"tool0=({tool[0]:.4f}, {tool[1]:.4f}), "
                f"target=({target_x:.4f}, {target_y:.4f}), "
                f"reachable={reachable}, "
                f"error={error:.8f}"
            )


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

    node = TwoLinkKinematicsVisualizer()

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

    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

저장:

Ctrl + O
Enter
Ctrl + X

코드를 큰 그림으로 읽으면:

• forward_kinematics() / inverse_kinematics() → 3편 Python 공식을 그대로 옮긴 FK/IK. IK는 cos(θ2)가 [-1, 1]을 벗어나면 None(도달 불가)을 돌려줍니다.
• demo_cases + publish_demo() → 5개 예제를 0.05초마다 발행하며, tick % 240 == 0일 때(약 12초마다) 다음 케이스로 전환합니다.
• publish_tf() → world→base_link→link1→link2→tool0 변환을 발행. 회전은 yaw_to_quaternion()으로 yaw를 quaternion(qz=sin(yaw/2), qw=cos(yaw/2))으로 바꿔 넣습니다(좌표 변환 개념은 W2D1 5편 참고).
• publish_markers() → 링크 선(robot_arm), 관절 구(base_joint·elbow_joint), 현재 tool0(빨강 tool0_current_position), 목표점(target_position — 도달 가능하면 초록, 불가능하면 빨강), 오차 선(tool_to_target_error), 작업공간 원(workspace_boundary)을 MarkerArray로 묶어 발행.
• print_case_result() → 매 tick % 80(약 4초)마다 케이스 결과를 로그로 찍어 터미널에서도 숫자를 확인할 수 있게 합니다.

💡 마커 색은 color(r, g, b, a)로 0~1 범위입니다.
손끝(tool0)은 빨강, 관절은 주황, 링크는 하늘색, 목표점은 도달 가능 초록/불가능 빨강, 작업공간 경계는 파란 원입니다.

---

9. setup.py 수정

무엇을: ros2 run으로 노드를 실행할 수 있게 진입점(entry point)을 등록합니다.

왜: entry_points에 console_scripts가 없으면 ros2 run이 노드를 찾지 못합니다.

cd ~/ros2_kinematics_ws/src/two_link_kinematics_demo
nano setup.py

전체 내용을 다음으로 교체합니다.

from setuptools import find_packages, setup

package_name = 'two_link_kinematics_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='Two-link kinematics visualization demo',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'two_link_kinematics_visualizer = two_link_kinematics_demo.two_link_kinematics_visualizer:main',
        ],
    },
)

저장:

Ctrl + O
Enter
Ctrl + X

💡 '실행이름 = 패키지.모듈:함수' 형식입니다.
여기서는 two_link_kinematics_visualizer라는 이름으로 two_link_kinematics_demo.two_link_kinematics_visualizer 모듈의 main 함수를 실행하도록 등록했습니다.
기대 결과: setup.py가 저장됩니다.

---

10. 패키지 빌드

무엇을: 패키지를 빌드하고 환경에 반영합니다. 왜: 빌드해야 ros2 run이 인식하고, source install/setup.bash로 이번 터미널에 패키지 경로를 알립니다.

cd ~/ros2_kinematics_ws
source /opt/ros/humble/setup.bash
colcon build --packages-select two_link_kinematics_demo
source install/setup.bash

⚠️ 빌드 후 source install/setup.bash를 빼먹으면 ros2 run이 패키지를 못 찾습니다.
새 터미널을 열 때마다 source /opt/ros/humble/setup.bash와 source install/setup.bash를 둘 다 해줘야 합니다.
기대 결과: Finished <<< two_link_kinematics_demo 로그와 함께 build/, install/, log/가 생성됩니다.

---

11. 화면 공유 실행

무엇을: 헤드리스 GUI 스택을 순서대로 띄웁니다(W2D1 3편 스택). 왜: GCP VM엔 모니터가 없으므로 가상 화면(Xvfb)→VNC(x11vnc)→웹(websockify)→외부 URL(Cloudflared)로 브라우저까지 화면을 보냅니다.

Xvfb :1 -screen 0 1280x800x24 > ~/xvfb.log 2>&1 &
DISPLAY=:1 fluxbox > ~/fluxbox.log 2>&1 &
x11vnc -display :1 -forever -shared -rfbport 5900 -nopw > ~/x11vnc.log 2>&1 &
websockify --web=/usr/share/novnc/ 6080 localhost:5900 > ~/websockify.log 2>&1 &
nohup cloudflared tunnel --url http://localhost:6080 > ~/tunnel.log 2>&1 &

💡 각 줄 끝의 &는 백그라운드 실행입니다. :1은 가상 디스플레이 번호, 5900은 VNC 포트, 6080은 웹 포트입니다. 스택의 원리·각 도구 역할은 W2D1 3편에 자세히 있습니다.
기대 결과: 5개 프로세스가 백그라운드로 뜨고 각 로그 파일이 생깁니다.

---

12. Cloudflared 주소 확인

무엇을: 외부 접속용 URL을 로그에서 확인합니다. 왜: Cloudflared가 매 실행마다 새 임시 URL을 발급하므로 로그에서 읽어야 합니다.

tail -n 20 ~/tunnel.log

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

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

⚠️ URL은 실행마다 바뀝니다. 접속이 안 되면 다시 tail ~/tunnel.log로 새 주소를 확인하세요. 끝에 반드시 /vnc.html 을 붙여야 noVNC 화면이 열립니다. 접속 후 Connect를 누르면 fluxbox 빈 화면이 보이고, 곧 RViz2가 그 위에 뜹니다.

---

13. RViz2 실행

무엇을: 7단계에서 만든 설정 파일로 RViz2를 가상 화면(:1)에 띄웁니다. 왜: -d로 설정 파일을 지정하면 Fixed Frame·Display가 자동으로 잡힙니다.

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

export DISPLAY=:1
export LIBGL_ALWAYS_SOFTWARE=1

rviz2 -d ~/ros2_kinematics_ws/rviz/two_link_kinematics.rviz > ~/rviz2.log 2>&1 &

⚠️ GPU 없는 VM 함정 — export LIBGL_ALWAYS_SOFTWARE=1을 빼면 RViz2가 GPU 가속을 시도하다 크래시하거나 검은 창이 됩니다. GCP 일반 VM엔 GPU가 없으므로 소프트웨어 렌더링을 강제해야 합니다.
또 export DISPLAY=:1을 빼면 RViz가 어느 화면에 그릴지 몰라 뜨지 않습니다.
기대 결과: 브라우저 noVNC 화면에 RViz2 창이 뜨고, 아직 데이터는 없습니다(노드 미실행) — Grid만 보입니다.

---

14. 시각화 노드 실행

무엇을: 8단계에서 작성한 노드를 실행합니다(터미널 2). 왜: 이 노드가 /tf와 /kinematics_demo/markers를 발행해야 RViz 화면에 팔이 그려집니다.

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

ros2 run two_link_kinematics_demo two_link_kinematics_visualizer

기대 결과: [INFO] ... Two-link kinematics visualizer started 로그가 뜨고, 이후 약 4초마다 print_case_result()가 케이스별 theta1, theta2, tool0, target, error를 출력합니다
(예: FK theta1=30 theta2=45 | theta1=30.00 deg, theta2=45.00 deg, tool0=(1.1248, 1.4659), ...). 이 터미널은 켜둡니다.

---

15. RViz2 화면 확인

브라우저 noVNC의 RViz2 화면에서 다음 요소들이 보이면 성공입니다.

Grid
TF
Kinematics Markers
world
base_link
link1
link2
tool0
robot_arm
base_joint
elbow_joint
tool0_current_position
target_position
tool_to_target_error
workspace_boundary

좌표축(TF) world→base_link→link1→link2→tool0, 하늘색 링크 선(robot_arm), 주황 관절 구(base_joint·elbow_joint), 빨간 손끝(tool0_current_position), 목표점(target_position), 손끝-목표 오차 선(tool_to_target_error), 파란 작업공간 원(workspace_boundary)이 함께 그려지고, 약 12초마다 5개 예제가 순서대로 바뀌며 팔 자세가 변합니다.

⚠️ 데이터가 안 보이면 — ① 노드(14단계)가 실행 중인지, ② RViz Fixed Frame이 world인지(7단계 함정), ③ Global Status가 OK인지 확인하세요. Frame [map] does not exist가 뜨면 Fixed Frame이 world가 아닌 것입니다.

---

16. Python 계산 결과와 RViz2 표시 결과 비교

이번 실습의 핵심입니다. 3편 Python 숫자와 RViz2 화면 위치가 일치하는지 확인합니다.

FK theta1=30 theta2=45
Python 계산 결과: x = 1.1248, y = 1.4659
RViz2 표시 위치: tool0_current_position

FK theta1=90 theta2=-90
Python 계산 결과: x = 1.0000, y = 1.0000
RViz2 표시 위치: tool0_current_position

IK target=(1.0, 1.0) solution A
Python 계산 결과: theta1 = 0도, theta2 = 90도
RViz2 표시 자세: base → (1, 0) → (1, 1)

IK target=(1.0, 1.0) solution B
Python 계산 결과: theta1 = 90도, theta2 = -90도
RViz2 표시 자세: base → (0, 1) → (1, 1)

IK target=(3.0, 0.0)
Python 계산 결과: no solution
RViz2 표시 결과: 도달 불가능 목표점

💡 IK 예제 1·2를 비교하면 같은 목표점 (1, 1) 에 도달하는 두 가지 자세(팔꿈치 위/아래)가 화면에서 서로 다른 모양으로 보입니다.
도달 불가능 예제((3.0, 0.0))는 작업공간 원(반지름 2.0) 바깥이라 목표점이 빨간색으로 표시되고, 팔은 기본 자세에 머뭅니다.
3편의 숫자가 4편의 그림으로 그대로 재현되는 것 — 이것이 이번 실습이 증명하려는 핵심입니다.

 

---

17. 토픽 확인

무엇을: 노드가 실제로 토픽을 발행하는지 확인합니다(터미널 3). 왜: RViz가 데이터를 못 받을 때, 발행 자체가 되는지부터 점검합니다.

ros2 topic list

/tf
/kinematics_demo/markers

발행 내용을 한 번씩 들여다보기:

ros2 topic echo /kinematics_demo/markers --once

ros2 topic echo /tf --once

💡 /kinematics_demo/markers에는 7개 마커가, /tf에는 world→base_link→...→tool0 변환이 담겨 있습니다.
토픽이 보이는데 RViz에 안 그려지면 거의 항상 Fixed Frame 문제입니다(7·15단계 함정).

---

18. 실습 종료

실습이 끝나면 노드·RViz와 화면 공유 스택을 정리합니다.

pkill -f "rviz2" || true
pkill -f "two_link_kinematics_visualizer" || true

화면 공유까지 종료합니다.

pkill -f "Xvfb :1" || true
pkill -f "fluxbox" || true
pkill -f "x11vnc" || true
pkill -f "websockify" || true
pkill -f "cloudflared" || true
fuser -k 5900/tcp || true
fuser -k 6080/tcp || true

💡 종료해 두면 다음 실습 때 4단계(프로세스·포트 정리)가 깨끗하게 통과합니다.

---

4편 정리

• 3편의 FK/IK Python 계산을 ROS2 노드(two_link_kinematics_visualizer.py)로 옮겨, /tf + /kinematics_demo/markers를 발행했습니다.
• .rviz 설정으로 Fixed Frame world + Grid·TF·MarkerArray를 고정하고, GCP VM 헤드리스 스택(Xvfb→x11vnc→websockify→Cloudflared)으로 브라우저에서 확인했습니다.
• 좌표계 트리는 world → base_link → link1 → link2 → tool0, 마커는 링크·관절·손끝·목표점·작업공간을 그렸습니다.
• 5개 예제에서 Python 숫자(tool0=(1.1248, 1.4659) 등) ↔ RViz 화면 위치가 일치함을 검증했습니다 — IK 두 해, 도달 불가 목표까지.
• 핵심 함정: Fixed Frame world(아니면 Frame [map] does not exist), LIBGL_ALWAYS_SOFTWARE=1(GPU 없는 VM), 빌드 후 source install/setup.bash.

다음 편 예고

지금까지는 우리가 직접 FK/IK를 계산해 자세를 그렸습니다.

하지만 실제 로봇 팔은 충돌 회피·관절 제한·궤적 생성까지 한꺼번에 풀어야 합니다.

5편부터는 이 모든 걸 통합하는 모션 플래닝 프레임워크 MoveIt 개념으로 들어갑니다 — Planning Group·Planning Scene·Collision·Trajectory의 핵심을 잡습니다.

---

시리즈 목차

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종)