659.69 렌치 (geometry_msgs::Wrench) 변환

659.69 렌치 (geometry_msgs::Wrench) 변환

1. 개요

렌치(wrench)는 강체에 작용하는 힘(force)과 토크(torque)를 동시에 표현하는 물리량이다. 수학적으로 렌치는 SE(3)의 쌍대 리 대수(dual Lie algebra) \mathfrak{se}^{*}(3)의 원소에 해당하며, 트위스트(twist)와 쌍대 관계(dual relationship)에 있다. 로봇 공학에서 렌치는 힘/토크 센서의 측정값, 접촉력 분석, 매니퓰레이터의 정적 해석, 그리고 임피던스 제어(impedance control) 등에서 핵심적으로 사용된다. TF2에서는 geometry_msgs::msg::Wrench 및 geometry_msgs::msg::WrenchStamped에 대한 좌표 변환을 tf2_geometry_msgs 패키지를 통해 지원한다.

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

2.1 Wrench

geometry_msgs/Wrench
  geometry_msgs/Vector3 force
    float64 x
    float64 y
    float64 z
  geometry_msgs/Vector3 torque
    float64 x
    float64 y
    float64 z

force 필드는 3차원 힘 벡터(N 단위)를 나타내며, torque 필드는 3차원 토크 벡터(N·m 단위)를 나타낸다.

2.2 WrenchStamped

geometry_msgs/WrenchStamped
  std_msgs/Header header
    builtin_interfaces/Time stamp
    string frame_id
  geometry_msgs/Wrench wrench

3. 렌치 변환의 수학적 원리

3.1 렌치의 좌표 변환

렌치 \mathcal{F} = (\boldsymbol{\tau}, \mathbf{f})^{\top}의 좌표 변환은 트위스트와 쌍대인 코어점트 수반 변환(coadjoint transformation)에 의해 정의된다. 프레임 A에서 프레임 B로의 변환 {}^{B}T_{A} = (R, \mathbf{t})에 대해,

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

코어점트 수반 행렬(coadjoint matrix) \text{Ad}^{*}_{T}는 다음과 같다.

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

이를 성분별로 분해하면,

힘 변환:
\mathbf{f}^{B} = R \cdot \mathbf{f}^{A}

토크 변환:
\boldsymbol{\tau}^{B} = R \cdot \boldsymbol{\tau}^{A} + \mathbf{t} \times (R \cdot \mathbf{f}^{A})

힘은 벡터량이므로 회전만 적용되지만, 토크는 작용점(point of application)의 변경에 따른 추가 모멘트가 발생한다. 추가 모멘트 \mathbf{t} \times (R \cdot \mathbf{f}^{A})는 힘의 작용점이 \mathbf{t}만큼 이동할 때 발생하는 모멘트 암(moment arm) 효과를 반영한다.

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

두 좌표 프레임의 원점이 일치하여 \mathbf{t} = \mathbf{0}인 경우,

\mathbf{f}^{B} = R \cdot \mathbf{f}^{A}, \quad \boldsymbol{\tau}^{B} = R \cdot \boldsymbol{\tau}^{A}

이 경우 힘과 토크 모두 단순한 벡터 회전만 적용된다.

3.3 트위스트와의 쌍대 관계

렌치와 트위스트는 쌍대 관계에 있으며, 두 물리량의 내적은 순간 동력(instantaneous power)을 산출한다.

P = \mathcal{F}^{\top} \cdot \mathcal{V} = \boldsymbol{\tau}^{\top} \cdot \boldsymbol{\omega} + \mathbf{f}^{\top} \cdot \mathbf{v}

이 쌍대성은 좌표 변환에서도 보존되어, 렌치의 변환이 트위스트 변환의 전치 역변환으로 정의되는 이유를 설명한다.

4. tf2_geometry_msgs에서의 렌치 변환

4.1 기본 변환 동작

tf2_geometry_msgs의 doTransform() 특수화는 렌치에 대해 힘과 토크 각각에 대해 회전만 적용하는 단순화된 변환을 수행한다. 코어점트 변환의 교차항 \mathbf{t} \times (R \cdot \mathbf{f})는 적용되지 않는다.

이 단순화된 변환은 힘/토크 센서가 센서 프레임에 설치되어 있고, 동일 원점의 다른 프레임으로 변환하는 경우(예: 센서 프레임 → 기체 프레임, 회전만 존재)에는 정확하다.

4.2 C++에서의 WrenchStamped 변환

#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

geometry_msgs::msg::WrenchStamped wrench_in;
wrench_in.header.frame_id = "ft_sensor_link";
wrench_in.header.stamp = node->now();
wrench_in.wrench.force.x = 0.0;
wrench_in.wrench.force.y = 0.0;
wrench_in.wrench.force.z = -9.81;  // 중력 방향 힘 (N)
wrench_in.wrench.torque.x = 0.1;
wrench_in.wrench.torque.y = -0.05;
wrench_in.wrench.torque.z = 0.0;

geometry_msgs::msg::WrenchStamped wrench_out;

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

4.3 Python에서의 Wrench 변환

from geometry_msgs.msg import WrenchStamped
import tf2_geometry_msgs

wrench_in = WrenchStamped()
wrench_in.header.frame_id = 'ft_sensor_link'
wrench_in.header.stamp = node.get_clock().now().to_msg()
wrench_in.wrench.force.z = -9.81
wrench_in.wrench.torque.x = 0.1

try:
    transform = tf_buffer.lookup_transform(
        'base_link', 'ft_sensor_link',
        wrench_in.header.stamp)
    wrench_out = tf2_geometry_msgs.do_transform_wrench(
        wrench_in, transform)
except TransformException as ex:
    node.get_logger().error(f'{ex}')

5. 완전한 코어점트 변환의 수동 구현

병진에 의한 토크 교차항이 필요한 경우, 수동으로 완전한 변환을 구현해야 한다.

void coadjointTransformWrench(
  const geometry_msgs::msg::WrenchStamped & wrench_in,
  geometry_msgs::msg::WrenchStamped & wrench_out,
  const geometry_msgs::msg::TransformStamped & transform)
{
  tf2::Transform T;
  tf2::fromMsg(transform.transform, T);
  
  tf2::Vector3 f_in(
    wrench_in.wrench.force.x,
    wrench_in.wrench.force.y,
    wrench_in.wrench.force.z);
  tf2::Vector3 tau_in(
    wrench_in.wrench.torque.x,
    wrench_in.wrench.torque.y,
    wrench_in.wrench.torque.z);
  
  tf2::Matrix3x3 R = T.getBasis();
  tf2::Vector3 t = T.getOrigin();
  
  // 힘 변환: f_B = R * f_A
  tf2::Vector3 f_out = R * f_in;
  
  // 토크 변환: tau_B = R * tau_A + t × (R * f_A)
  tf2::Vector3 tau_out = R * tau_in + t.cross(f_out);
  
  wrench_out.header = transform.header;
  wrench_out.wrench.force.x = f_out.x();
  wrench_out.wrench.force.y = f_out.y();
  wrench_out.wrench.force.z = f_out.z();
  wrench_out.wrench.torque.x = tau_out.x();
  wrench_out.wrench.torque.y = tau_out.y();
  wrench_out.wrench.torque.z = tau_out.z();
}

6. 응용 사례

6.1 힘/토크 센서 데이터 변환

매니퓰레이터의 손목(wrist)에 설치된 6축 힘/토크 센서(F/T sensor)에서 측정된 렌치를 기체 프레임이나 세계 프레임으로 변환하여 중력 보상(gravity compensation)이나 접촉력 분석에 활용한다.

6.2 임피던스 제어

임피던스 제어에서는 원하는 렌치와 실제 측정 렌치 간의 차이를 계산하여 제어 입력을 산출한다. 이때 두 렌치가 동일한 좌표 프레임에서 표현되어야 하므로 렌치 변환이 필요하다.

6.3 정적 평형 해석

로봇의 정적 평형 조건은 렌치의 총합이 0이 되는 것이다. 서로 다른 프레임에서 표현된 힘과 토크를 단일 프레임으로 변환한 후 합산하여 평형을 분석한다.

6.4 중력 보상

힘/토크 센서 측정값에서 센서 이후 링크 및 부착물의 중력 성분을 제거(gravity compensation)할 때, 중력 방향 벡터를 센서 프레임으로 변환하여 중력에 의한 렌치를 계산하고 차감한다.

7. 주의사항

7.1 작용점의 변경

렌치의 좌표 변환에서 가장 중요한 점은, 두 좌표 프레임의 원점이 다른 경우 토크 성분에 추가 모멘트가 발생한다는 것이다. 이를 무시하면 토크 변환 결과가 물리적으로 부정확해진다. 특히 센서 프레임과 관심 프레임 사이의 병진이 유의미한 경우에 반드시 교차항을 고려해야 한다.

7.2 렌치의 크기 보존

힘 벡터와 토크 벡터 각각의 크기(norm)는 순수 회전 변환에서 보존된다. 그러나 코어점트 변환의 교차항에 의해 토크의 크기는 변환 전후로 달라질 수 있다. 이는 물리적으로 올바른 현상이며, 작용점의 변경에 따른 모멘트 변화를 반영한다.

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.
  • B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics: Modelling, Planning and Control, Springer, 2009.

버전: 2026-03-26