659.68 트위스트 (geometry_msgs::Twist) 변환

659.68 트위스트 (geometry_msgs::Twist) 변환

1. 개요

트위스트(twist)는 강체의 순간 속도를 나타내는 물리량으로, 선속도(linear velocity) \mathbf{v} \in \mathbb{R}^{3}와 각속도(angular velocity) \boldsymbol{\omega} \in \mathbb{R}^{3}의 쌍으로 구성된다. 수학적으로 트위스트는 SE(3)의 리 대수(Lie algebra) \mathfrak{se}(3)의 원소에 해당하며, 로봇의 운동 상태를 기술하는 핵심 물리량이다. TF2에서 geometry_msgs::msg::Twist 메시지의 좌표 변환은 선속도와 각속도의 기하학적 특성을 고려한 변환이 적용된다.

2. geometry_msgs::msg::Twist 메시지 구조

2.1 Twist

geometry_msgs/Twist
  geometry_msgs/Vector3 linear
    float64 x
    float64 y
    float64 z
  geometry_msgs/Vector3 angular
    float64 x
    float64 y
    float64 z

linear 필드는 선속도(m/s)를 나타내며, angular 필드는 각속도(rad/s)를 나타낸다. 두 필드 모두 해당 좌표 프레임의 x, y, z 축 방향 성분으로 표현된다.

2.2 TwistStamped

geometry_msgs/TwistStamped
  std_msgs/Header header
    builtin_interfaces/Time stamp
    string frame_id
  geometry_msgs/Twist twist

2.3 TwistWithCovariance

geometry_msgs/TwistWithCovariance
  geometry_msgs/Twist twist
  float64[36] covariance

covariance는 6 \times 6 공분산 행렬로, 처음 3개 차원은 선속도의 불확실성, 나머지 3개 차원은 각속도의 불확실성을 나타낸다.

3. 트위스트 변환의 수학적 원리

3.1 기체 좌표계에서의 트위스트 (Body Twist)

로봇 공학에서 트위스트는 일반적으로 기체 좌표계(body frame)에서 표현되는 기체 트위스트(body twist)와 공간 좌표계(spatial frame)에서 표현되는 공간 트위스트(spatial twist)로 구분된다.

기체 트위스트 \mathcal{V}^{b}는 해당 강체에 고정된 좌표계에서 표현된 속도이다.

\mathcal{V}^{b} = \begin{bmatrix} \boldsymbol{\omega}^{b} \\ \mathbf{v}^{b} \end{bmatrix} \in \mathbb{R}^{6}

3.2 트위스트의 좌표 변환

프레임 A에서 표현된 트위스트를 프레임 B에서 표현하려면, 수반 변환(adjoint transformation)을 적용해야 한다.

\mathcal{V}^{B} = \text{Ad}_{{}^{B}T_{A}} \cdot \mathcal{V}^{A}

여기서 수반 행렬(adjoint matrix) \text{Ad}_{T}는 6 \times 6 행렬로, 변환 행렬 T = (R, \mathbf{t})에 대해 다음과 같이 정의된다.

\text{Ad}_{T} = \begin{bmatrix} R & \mathbf{0} \\ [\mathbf{t}]_{\times} R & R \end{bmatrix}

여기서 [\mathbf{t}]_{\times}는 벡터 \mathbf{t}의 반대칭 행렬(skew-symmetric matrix)이다.

[\mathbf{t}]_{\times} = \begin{bmatrix} 0 & -t_z & t_y \\ t_z & 0 & -t_x \\ -t_y & t_x & 0 \end{bmatrix}

3.3 성분별 변환

수반 변환을 성분별로 분해하면 다음과 같다.

각속도 변환:
\boldsymbol{\omega}^{B} = R \cdot \boldsymbol{\omega}^{A}

선속도 변환:
\mathbf{v}^{B} = R \cdot \mathbf{v}^{A} + [\mathbf{t}]_{\times} R \cdot \boldsymbol{\omega}^{A} = R \cdot \mathbf{v}^{A} + \mathbf{t} \times (R \cdot \boldsymbol{\omega}^{A})

각속도는 벡터와 동일하게 회전만 적용되지만, 선속도는 회전에 더하여 각속도에 의한 추가 속도 성분이 발생한다. 이는 좌표 프레임의 원점이 이동하면(병진이 존재하면) 회전에 의한 원심 속도(centripetal velocity) 성분이 발생하기 때문이다.

3.4 단순화된 변환 (원점이 동일한 경우)

두 좌표 프레임의 원점이 일치하여 \mathbf{t} = \mathbf{0}인 경우, 수반 변환은 다음과 같이 단순화된다.

\boldsymbol{\omega}^{B} = R \cdot \boldsymbol{\omega}^{A}, \quad \mathbf{v}^{B} = R \cdot \mathbf{v}^{A}

이 경우 선속도와 각속도 모두 단순한 벡터 회전만 적용된다.

4. tf2_geometry_msgs에서의 트위스트 변환

4.1 현재 지원 상태

tf2_geometry_msgs에서 TwistStamped에 대한 doTransform() 특수화는 선속도와 각속도 각각에 대해 회전만 적용하는 단순화된 변환을 수행한다. 즉, 수반 변환의 교차항 [\mathbf{t}]_{\times} R \cdot \boldsymbol{\omega}는 적용되지 않는다.

이는 트위스트가 기체 좌표계에서 표현된 기체 트위스트로 해석되는 일반적인 로봇 응용 시나리오에서는 올바른 결과를 산출하지만, 두 프레임 간의 병진이 유의미하고 각속도가 존재하는 경우에는 물리적으로 정확한 변환이 되지 않을 수 있다.

4.2 C++에서의 TwistStamped 변환

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

geometry_msgs::msg::TwistStamped twist_in;
twist_in.header.frame_id = "base_link";
twist_in.header.stamp = node->now();
twist_in.twist.linear.x = 1.0;   // 전방 1.0 m/s
twist_in.twist.linear.y = 0.0;
twist_in.twist.linear.z = 0.0;
twist_in.twist.angular.x = 0.0;
twist_in.twist.angular.y = 0.0;
twist_in.twist.angular.z = 0.5;  // 좌회전 0.5 rad/s

geometry_msgs::msg::TwistStamped twist_out;

try {
  auto transform = tf_buffer->lookupTransform(
    "odom", "base_link", twist_in.header.stamp);
  tf2::doTransform(twist_in, twist_out, transform);
} catch (const tf2::TransformException & ex) {
  RCLCPP_ERROR(node->get_logger(), "%s", ex.what());
}

4.3 Python에서의 Twist 변환

from geometry_msgs.msg import TwistStamped
import tf2_geometry_msgs

twist_in = TwistStamped()
twist_in.header.frame_id = 'base_link'
twist_in.header.stamp = node.get_clock().now().to_msg()
twist_in.twist.linear.x = 1.0
twist_in.twist.angular.z = 0.5

try:
    transform = tf_buffer.lookup_transform(
        'odom', 'base_link',
        twist_in.header.stamp)
    twist_out = tf2_geometry_msgs.do_transform_twist_stamped(
        twist_in, transform)
except TransformException as ex:
    node.get_logger().error(f'{ex}')

5. 수반 변환의 수동 구현

5.1 완전한 수반 변환이 필요한 경우

tf2_geometry_msgs의 기본 트위스트 변환이 수반 변환의 교차항을 포함하지 않으므로, 물리적으로 정확한 변환이 필요한 경우 수동으로 구현해야 한다.

#include <tf2/LinearMath/Transform.h>
#include <tf2/LinearMath/Vector3.h>

void adjointTransformTwist(
  const geometry_msgs::msg::TwistStamped & twist_in,
  geometry_msgs::msg::TwistStamped & twist_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  tf2::Transform T;
  tf2::fromMsg(transform.transform, T);
  
  tf2::Vector3 omega_in(
    twist_in.twist.angular.x,
    twist_in.twist.angular.y,
    twist_in.twist.angular.z);
  tf2::Vector3 v_in(
    twist_in.twist.linear.x,
    twist_in.twist.linear.y,
    twist_in.twist.linear.z);
  
  tf2::Matrix3x3 R = T.getBasis();
  tf2::Vector3 t = T.getOrigin();
  
  // 각속도 변환: omega_B = R * omega_A
  tf2::Vector3 omega_out = R * omega_in;
  
  // 선속도 변환: v_B = R * v_A + t × (R * omega_A)
  tf2::Vector3 v_out = R * v_in + t.cross(omega_out);
  
  twist_out.header = transform.header;
  twist_out.twist.linear.x = v_out.x();
  twist_out.twist.linear.y = v_out.y();
  twist_out.twist.linear.z = v_out.z();
  twist_out.twist.angular.x = omega_out.x();
  twist_out.twist.angular.y = omega_out.y();
  twist_out.twist.angular.z = omega_out.z();
}

6. Twist의 사용 패턴

6.1 cmd_vel 토픽

이동 로봇의 속도 명령은 관례적으로 /cmd_vel 토픽을 통해 geometry_msgs::msg::Twist로 발행된다. 이 경우 트위스트는 로봇 기체 프레임(base_link) 기준이며, TwistStamped가 아닌 Twist(Non-Stamped)가 사용된다.

6.2 오도메트리 (Odometry)

nav_msgs::msg::Odometry 메시지에는 TwistWithCovariance가 포함되어 있으며, 일반적으로 기체 프레임(child_frame_id)에서의 속도를 나타낸다.

6.3 매니퓰레이터 관절 속도

매니퓰레이터의 말단 속도(end-effector velocity)는 시공간 트위스트 또는 기체 트위스트로 표현되며, 야코비안(Jacobian) 행렬을 통해 관절 속도와 연계된다.

7. 주의사항

7.1 Twist vs TwistStamped

geometry_msgs::msg::Twist(Non-Stamped)는 프레임 정보가 없으므로 Buffer::transform()을 사용할 수 없다. TF2 기반 변환을 위해서는 TwistStamped를 사용하거나, 수동으로 프레임 정보를 관리해야 한다.

7.2 변환의 물리적 정확성

기본 doTransform() 특수화가 수반 변환의 교차항을 포함하지 않으므로, 병진이 유의미한 두 프레임 간의 트위스트 변환에서는 결과의 물리적 정확성을 검증해야 한다. 동일 원점의 프레임 간(예: 회전만 다른 두 프레임) 변환에서는 기본 doTransform()으로 충분하다.

7.3 기체 트위스트와 공간 트위스트

동일한 물리적 운동에 대해 기체 트위스트와 공간 트위스트는 서로 다른 수치를 가진다. 트위스트를 사용할 때는 어떤 규약이 사용되고 있는지 명확히 파악해야 한다. ROS2에서는 관례적으로 기체 프레임 기준의 트위스트가 사용된다.

8. 참고 문헌

  • R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994.
  • K. M. Lynch and F. C. Park, Modern Robotics: Mechanics, Planning, and Control, Cambridge University Press, 2017.
  • Open Robotics, “geometry_msgs/Twist,” ROS 2 Documentation, https://docs.ros.org/en/rolling/p/geometry_msgs/

버전: 2026-03-26