TF2에서의 좌표 변환 예제

1. 기본 좌표 변환 개념

TF2는 ROS2에서 좌표 변환을 관리하는 패키지로, 각 좌표계 간의 변환을 처리한다. 로봇 시스템에서 다양한 센서나 모터, 로봇 팔 등이 각기 다른 좌표계를 가질 수 있기 때문에 좌표계 간 변환은 필수적이다. 변환은 주로 회전 행렬과 변위 벡터로 이루어지며, 이를 통해 로봇의 위치나 방향을 다른 좌표계로 변환할 수 있다.

좌표 변환은 주로 4x4 변환 행렬을 사용하여 이루어지며, 이를 통해 회전과 변위를 한 번에 표현할 수 있다. 변환 행렬은 다음과 같이 구성된다.

$\mathbf{T} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ 0 & 1 \end{bmatrix}$

여기서: - $\mathbf{T}$ 는 4x4 변환 행렬 - $\mathbf{R}$ 은 3x3 회전 행렬 - $\mathbf{t}$ 는 3x1 변위 벡터 (평행 이동) - 아래의 $0$ 과 $1$ 은 행렬의 크기를 맞추기 위한 값이다.

2. 회전 행렬과 변위 벡터

회전 행렬 $\mathbf{R}$ 는 로봇이 회전하는 방향을 나타내며, 변위 벡터 $\mathbf{t}$ 는 좌표계를 평행 이동시키는 역할을 한다. 3차원 공간에서의 회전은 주로 Euler 각이나 회전 행렬, 쿼터니언으로 표현된다.

3차원 회전 행렬 $\mathbf{R}$ 는 다음과 같이 나타낼 수 있다.

$\mathbf{R} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix}$

변위 벡터 $\mathbf{t}$ 는 다음과 같이 나타낼 수 있다.

$\mathbf{t} = \begin{bmatrix} t_x \\ t_y \\ t_z \end{bmatrix}$

따라서, 변환 행렬 $\mathbf{T}$ 는 회전과 평행 이동을 통합하여 다음과 같이 나타낼 수 있다.

$\mathbf{T} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \\ 0 & 0 & 0 & 1 \end{bmatrix}$

이 행렬을 사용하면 좌표계 A에서의 한 점 $\mathbf{p}_A = (x_A, y_A, z_A)$ 를 좌표계 B로 변환할 수 있다. 변환된 좌표계 B에서의 점 $\mathbf{p}_B$ 는 다음과 같이 계산된다.

$\begin{bmatrix} x_B \\ y_B \\ z_B \\ 1 \end{bmatrix} = \mathbf{T} \begin{bmatrix} x_A \\ y_A \\ z_A \\ 1 \end{bmatrix}$

이를 풀어서 쓰면 다음과 같다.

$\begin{aligned} x_B &= r_{11}x_A + r_{12}y_A + r_{13}z_A + t_x \\ y_B &= r_{21}x_A + r_{22}y_A + r_{23}z_A + t_y \\ z_B &= r_{31}x_A + r_{32}y_A + r_{33}z_A + t_z \end{aligned}$

이 수식은 좌표계 A에서의 점을 좌표계 B로 변환할 때 사용하는 기본적인 변환 공식이다.

3. ROS2에서의 TF2 좌표 변환 예제

ROS2에서 TF2를 사용해 두 좌표계 간 변환을 수행하는 예제를 살펴보겠다. TF2에서는 두 좌표계 간의 관계를 트랜스폼(Transform)으로 정의하며, 이는 시간에 따라 변할 수 있는 동적 정보를 포함한다.

먼저, Python을 사용하여 TF2 변환을 구현하는 기본적인 방법은 다음과 같다.

import tf2_ros
import geometry_msgs.msg

# TF2 버퍼와 리스너 초기화
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)

# 변환을 시도할 좌표계 설정
source_frame = 'base_link'
target_frame = 'camera_link'

# 현재 시간을 기준으로 변환 요청
try:
    trans = tf_buffer.lookup_transform(target_frame, source_frame, rospy.Time(0))
    print("Transform: ", trans)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
    print("Transform not available.")

이 코드는 두 좌표계 'base_link'와 'camera_link' 간의 변환을 요청하는 예제이다. TF2는 시간에 따라 동적으로 변할 수 있는 트랜스폼을 처리하기 때문에, 변환을 요청할 때 시간을 명시할 수 있다. 여기서는 현재 시점의 트랜스폼을 요청하고 있다.

이후의 변환 과정을 위해 트랜스폼의 자세한 내용을 살펴보면, 트랜스폼은 변위와 회전 정보를 포함한다. ROS2에서는 이 트랜스폼 데이터를 활용하여 좌표계 간의 변환을 처리한다.

4. 트랜스폼 데이터에서 회전 및 변위 추출

위의 예제에서 lookup_transform 함수를 통해 얻은 트랜스폼 객체에는 좌표계 간의 변위와 회전 정보가 포함되어 있다. 이 정보를 활용하여 좌표 변환을 수행할 수 있다.

트랜스폼 객체는 다음과 같은 형식으로 변위와 회전 데이터를 포함한다.

trans.transform.translation.x
trans.transform.translation.y
trans.transform.translation.z

trans.transform.rotation.x
trans.transform.rotation.y
trans.transform.rotation.z
trans.transform.rotation.w

이 데이터 중 변위는 translation에 저장되며, 회전 정보는 쿼터니언 형태로 rotation에 저장된다. 쿼터니언은 회전을 나타내는 방식 중 하나로, 기울기(roll), 피치(pitch), 요(roll)를 회전 행렬보다 효율적으로 계산할 수 있다.

변환을 통한 좌표 변환 예제

트랜스폼 객체에서 변위와 회전 정보를 추출한 후, 이를 이용하여 좌표를 변환하는 방법을 예제로 보여드리겠다. 예를 들어, 좌표계 A에서의 한 점 $\mathbf{p}_A = (x_A, y_A, z_A)$ 를 좌표계 B로 변환하려면 다음과 같은 절차를 따른다.

변위 벡터 $\mathbf{t}$ 추출:

t_x = trans.transform.translation.x
t_y = trans.transform.translation.y
t_z = trans.transform.translation.z

이를 통해 변위 벡터 $\mathbf{t} = (t_x, t_y, t_z)$ 를 추출할 수 있다.

쿼터니언을 회전 행렬로 변환:

쿼터니언을 회전 행렬로 변환하려면 추가적인 라이브러리나 수식을 사용할 수 있다. 여기서는 tf_transformations 라이브러리를 사용하여 쿼터니언을 회전 행렬로 변환하는 방법을 보여드린다.

import tf_transformations

# 쿼터니언 값 추출
q_x = trans.transform.rotation.x
q_y = trans.transform.rotation.y
q_z = trans.transform.rotation.z
q_w = trans.transform.rotation.w

# 쿼터니언을 회전 행렬로 변환
rotation_matrix = tf_transformations.quaternion_matrix([q_x, q_y, q_z, q_w])

이때 변환된 회전 행렬은 4x4 행렬 형태로 반환되며, 이는 앞서 설명한 $\mathbf{R}$ 을 포함한다.

5. 점의 변환

이제 좌표계 A에서의 점 $\mathbf{p}_A = (x_A, y_A, z_A)$ 를 좌표계 B로 변환하기 위해 변위 벡터와 회전 행렬을 사용하여 다음과 같은 수식을 적용한다.

우선, 점 $\mathbf{p}_A$ 를 동차 좌표계로 표현한 후, 변환 행렬을 적용한다.

$\mathbf{p}_A = \begin{bmatrix} x_A \\ y_A \\ z_A \\ 1 \end{bmatrix}$

변환된 좌표계 B에서의 점 $\mathbf{p}_B$ 는 다음과 같이 계산된다.

$\mathbf{p}_B = \mathbf{T} \cdot \mathbf{p}_A$

변환 행렬 $\mathbf{T}$ 는 회전 행렬 $\mathbf{R}$ 과 변위 벡터 $\mathbf{t}$ 로 이루어져 있으며, 앞서 설명한 4x4 변환 행렬 형식을 사용한다.

코드로 표현한 좌표 변환

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

import numpy as np

# 변위 벡터 및 회전 행렬
translation = np.array([t_x, t_y, t_z])
rotation_matrix = rotation_matrix[:3, :3]  # 3x3 회전 행렬 추출

# 좌표계 A에서의 점
point_A = np.array([x_A, y_A, z_A])

# 좌표계 B로 변환
point_B = np.dot(rotation_matrix, point_A) + translation

위 코드는 좌표계 A에서 좌표계 B로 변환하는 과정을 보여준다. 회전 행렬과 변위 벡터를 사용하여 좌표 변환을 수행하고, 결과적으로 변환된 좌표계를 계산한다.

6. TF2에서의 좌표 변환 체인

좌표 변환은 단일 좌표계 간의 변환뿐만 아니라 여러 좌표계를 거치는 복잡한 변환도 필요할 수 있다. 예를 들어, 로봇 시스템에서 로봇 베이스(base_link)에서 카메라(camera_link), 그리고 센서 좌표계(sensor_link)까지 변환이 필요할 수 있다. 이런 경우 TF2는 변환 체인을 자동으로 처리하여, 중간 좌표계를 거쳐 최종 변환을 수행한다.

변환 체인의 개념

여러 좌표계를 변환하는 경우, 변환 체인은 각 좌표계 간의 관계를 연결한 트리 구조를 형성한다. 예를 들어, 다음과 같은 변환 체인을 가정할 수 있다:

$\mathbf{T}_{\text{camera\_link}}^{\text{base\_link}} \rightarrow \mathbf{T}_{\text{sensor\_link}}^{\text{camera\_link}}$

이 경우, 로봇의 베이스 좌표계에서 카메라 좌표계를 거쳐 센서 좌표계까지 변환하는 과정을 하나의 체인으로 처리할 수 있다. 이 변환은 TF2의 lookup_transform 기능을 사용하여 자동으로 처리된다.

코드 예제

다음은 ROS2에서 변환 체인을 처리하는 예제이다.

# 여러 좌표계를 거쳐 변환 요청
try:
    # base_link에서 sensor_link까지의 변환 체인
    trans = tf_buffer.lookup_transform('sensor_link', 'base_link', rospy.Time(0))
    print("Transform Chain: ", trans)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
    print("Transform chain not available.")

이 코드는 base_link에서 sensor_link까지의 변환을 요청하며, TF2는 중간 좌표계(camera_link)를 자동으로 처리한다. 중간 변환들을 수동으로 계산할 필요 없이, TF2가 알아서 중간 변환을 연결하여 최종 변환을 수행한다.

7. TF2에서 좌표 변환 체인의 시각화

복잡한 변환 체인은 TF2에서 트리 구조로 표현되며, 각 좌표계 간의 관계를 시각화할 수 있다. 이를 위해 rqt_tf_tree 또는 tf_monitor 명령어를 사용할 수 있다.

tf_monitor 명령어 사용

ros2 run tf2_tools view_frames

이 명령을 실행하면 TF2의 좌표 변환 트리를 시각화한 PDF 파일을 생성할 수 있다. 이를 통해 각 좌표계 간의 변환 관계를 쉽게 파악할 수 있다.

TF2 트리의 예시

다이어그램을 활용하여 간단한 좌표 변환 체인을 시각화하면 다음과 같다:

graph LR A[base_link] --> B[camera_link] B --> C[sensor_link]

이 구조는 로봇의 베이스 좌표계에서 카메라, 그리고 센서 좌표계까지의 변환 체인을 간단히 나타낸다.

8. 쿼터니언을 이용한 회전 처리

앞서 설명한 좌표 변환에서 회전은 중요한 요소 중 하나이며, 회전을 표현하는 방법에는 여러 가지가 있다. 대표적인 방법은 Euler 각과 쿼터니언이다. TF2에서는 주로 쿼터니언을 사용하여 회전을 표현한다. 쿼터니언은 특이점(singularity)이 발생하지 않으며, 연속적인 회전을 효율적으로 계산할 수 있는 장점이 있다.

쿼터니언의 정의

쿼터니언은 다음과 같이 4개의 요소로 정의된다.

$\mathbf{q} = \left( q_x, q_y, q_z, q_w \right)$

여기서: - $q_x, q_y, q_z$ 는 회전축을 나타내는 벡터의 성분이며, - $q_w$ 는 회전각의 코사인 값과 관련된 요소이다.

쿼터니언은 다음과 같은 수식을 통해 회전 행렬로 변환할 수 있다.

$\mathbf{R} = \begin{bmatrix} 1 - 2(q_y^2 + q_z^2) & 2(q_x q_y - q_z q_w) & 2(q_x q_z + q_y q_w) \\ 2(q_x q_y + q_z q_w) & 1 - 2(q_x^2 + q_z^2) & 2(q_y q_z - q_x q_w) \\ 2(q_x q_z - q_y q_w) & 2(q_y q_z + q_x q_w) & 1 - 2(q_x^2 + q_y^2) \end{bmatrix}$

이 회전 행렬을 이용하여 쿼터니언으로부터 3D 공간에서의 회전을 계산할 수 있다.

9. 쿼터니언 회전 예제

ROS2의 TF2 패키지에서는 쿼터니언을 쉽게 사용할 수 있다. 앞서 언급한 Python 코드 예제를 확장하여 쿼터니언 회전을 적용해보겠다.

import tf_transformations

# 쿼터니언 값 추출
q_x = trans.transform.rotation.x
q_y = trans.transform.rotation.y
q_z = trans.transform.rotation.z
q_w = trans.transform.rotation.w

# 쿼터니언을 회전 행렬로 변환
rotation_matrix = tf_transformations.quaternion_matrix([q_x, q_y, q_z, q_w])

# 회전 행렬 출력
print("Rotation Matrix: \n", rotation_matrix)

위 코드에서 쿼터니언을 이용해 회전 행렬을 계산하고, 이를 이용해 좌표계 간의 변환을 처리할 수 있다.

10. 쿼터니언을 이용한 좌표 변환

쿼터니언을 이용한 좌표 변환은 변환 행렬을 사용하는 것과 동일한 원리로 진행된다. 쿼터니언을 회전 행렬로 변환한 후, 변위 벡터와 함께 사용하여 좌표 변환을 처리한다. 예를 들어, 좌표계 A에서 좌표계 B로의 변환을 수행할 때, 쿼터니언으로부터 얻은 회전 행렬과 변위 벡터를 사용하여 변환된 좌표를 계산할 수 있다.

import numpy as np

# 변위 벡터 및 회전 행렬
translation = np.array([t_x, t_y, t_z])
rotation_matrix = rotation_matrix[:3, :3]  # 3x3 회전 행렬 추출

# 좌표계 A에서의 점
point_A = np.array([x_A, y_A, z_A])

# 좌표계 B로 변환
point_B = np.dot(rotation_matrix, point_A) + translation
print("Transformed Point in B: ", point_B)

11. 좌표 변환의 실시간 처리

ROS2의 TF2는 좌표 변환을 실시간으로 처리할 수 있다. 특히 로봇의 움직임이나 센서 데이터가 시간에 따라 변화할 때, 실시간으로 좌표계를 변환하여 처리하는 것이 중요하다. TF2는 이와 같은 실시간 변환을 효율적으로 지원하며, 노드 간 통신을 통해 동적으로 좌표 변환 정보를 업데이트할 수 있다.

실시간 변환 처리를 위해서는 TF2의 lookup_transform 메소드를 반복적으로 호출하여 변환 정보를 갱신하고, 최신 좌표계를 기반으로 변환을 수행해야 한다. 예를 들어, 로봇이 이동하면서 좌표계가 지속적으로 변하는 상황에서는 변환 정보를 실시간으로 반영하여 로봇의 위치나 방향을 추적할 수 있다.

실시간 좌표 변환 예제

다음은 실시간으로 좌표 변환을 처리하는 예제이다.

import rospy
import tf2_ros
import geometry_msgs.msg

def get_transform():
    tf_buffer = tf2_ros.Buffer()
    tf_listener = tf2_ros.TransformListener(tf_buffer)

    while not rospy.is_shutdown():
        try:
            # 실시간으로 좌표 변환 요청
            trans = tf_buffer.lookup_transform('sensor_link', 'base_link', rospy.Time(0))
            print("Real-time Transform: ", trans)
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            rospy.logwarn("Transform not available")

        rospy.sleep(0.1)  # 100ms 간격으로 변환 요청

if __name__ == '__main__':
    rospy.init_node('real_time_tf_listener')
    get_transform()

위 코드는 매 100ms마다 변환 정보를 요청하여 실시간으로 좌표 변환을 처리한다. 이를 통해 로봇의 움직임이나 센서 데이터의 변화를 추적할 수 있다.