659.74 tf2_kdl을 이용한 KDL 라이브러리 연동

1. 개요

KDL(Kinematics and Dynamics Library)은 Orocos(Open Robot Control Software) 프로젝트의 일부로 개발된 C++ 기반 기구학 및 동역학 라이브러리이다. KDL은 로봇 매니퓰레이터의 순기구학(forward kinematics), 역기구학(inverse kinematics), 야코비안(Jacobian) 계산, 역동역학(inverse dynamics) 등 로봇 기구학·동역학의 핵심 알고리즘을 제공한다. tf2_kdl 패키지는 TF2의 변환 타입과 KDL의 기하학적 타입 간의 양방향 변환을 제공하며, 기구학 계산 결과를 TF2 좌표 변환 시스템과 통합하는 데 활용된다.

2. KDL의 핵심 기하학적 타입

2.1 KDL::Vector

3차원 벡터를 나타내며, 위치, 힘, 속도 등의 표현에 사용된다.

KDL::Vector v(1.0, 2.0, 3.0);
double x = v.x();
double y = v.y();
double z = v.z();

2.2 KDL::Rotation

3 \times 3 회전 행렬을 나타낸다. 다양한 생성 방법을 지원한다.

// RPY (Roll-Pitch-Yaw)로 생성
KDL::Rotation R = KDL::Rotation::RPY(roll, pitch, yaw);

// 축-각(Axis-Angle)으로 생성
KDL::Rotation R2 = KDL::Rotation::Rot(KDL::Vector(0, 0, 1), M_PI / 4);

// 쿼터니언으로 생성
KDL::Rotation R3 = KDL::Rotation::Quaternion(x, y, z, w);

// 항등 회전
KDL::Rotation R_id = KDL::Rotation::Identity();

2.3 KDL::Frame

위치와 자세를 결합한 참조 프레임(강체 변환)을 나타낸다. SE(3)의 원소에 해당한다.

// 회전 + 병진으로 생성
KDL::Frame F(KDL::Rotation::RPY(0, 0, M_PI / 4),
             KDL::Vector(1.0, 2.0, 0.0));

// 항등 프레임
KDL::Frame F_id = KDL::Frame::Identity();

// 프레임의 역변환
KDL::Frame F_inv = F.Inverse();

// 프레임 합성
KDL::Frame F_composed = F1 * F2;

2.4 KDL::Twist

선속도와 각속도의 쌍으로 강체의 순간 속도를 나타낸다.

KDL::Twist twist(
  KDL::Vector(1.0, 0.0, 0.0),   // 선속도
  KDL::Vector(0.0, 0.0, 0.5));  // 각속도

2.5 KDL::Wrench

힘과 토크의 쌍으로 강체에 작용하는 렌치를 나타낸다.

KDL::Wrench wrench(
  KDL::Vector(0.0, 0.0, -9.81),  // 힘
  KDL::Vector(0.1, 0.0, 0.0));   // 토크

3. tf2_kdl의 타입 변환

3.1 의존성 설정

find_package(tf2_kdl REQUIRED)
find_package(orocos_kdl REQUIRED)

ament_target_dependencies(${PROJECT_NAME}
  tf2
  tf2_ros
  tf2_kdl
  orocos_kdl
)

<depend>tf2_kdl</depend>
<depend>orocos_kdl</depend>

3.2 헤더 포함

#include <tf2_kdl/tf2_kdl.hpp>
#include <kdl/frames.hpp>

4. KDL ↔ ROS 메시지 변환

4.1 KDL::Frame ↔ geometry_msgs::msg::Pose

#include <tf2_kdl/tf2_kdl.hpp>

// KDL::Frame → geometry_msgs::msg::Pose
KDL::Frame kdl_frame(
  KDL::Rotation::RPY(0.0, 0.0, M_PI / 4),
  KDL::Vector(1.0, 2.0, 0.0));

geometry_msgs::msg::Pose ros_pose;
tf2::toMsg(kdl_frame, ros_pose);

// geometry_msgs::msg::Pose → KDL::Frame
KDL::Frame kdl_frame_out;
tf2::fromMsg(ros_pose, kdl_frame_out);

4.2 KDL::Vector ↔ geometry_msgs::msg::Point

// KDL::Vector → geometry_msgs::msg::Point
KDL::Vector kdl_vec(1.0, 2.0, 3.0);
geometry_msgs::msg::Point ros_point;
tf2::toMsg(kdl_vec, ros_point);

// geometry_msgs::msg::Point → KDL::Vector
KDL::Vector kdl_vec_out;
tf2::fromMsg(ros_point, kdl_vec_out);

4.3 KDL::Twist ↔ geometry_msgs::msg::Twist

// KDL::Twist → geometry_msgs::msg::Twist
KDL::Twist kdl_twist(
  KDL::Vector(1.0, 0.0, 0.0),
  KDL::Vector(0.0, 0.0, 0.5));
geometry_msgs::msg::Twist ros_twist;
tf2::toMsg(kdl_twist, ros_twist);

// geometry_msgs::msg::Twist → KDL::Twist
KDL::Twist kdl_twist_out;
tf2::fromMsg(ros_twist, kdl_twist_out);

4.4 KDL::Frame ↔ geometry_msgs::msg::TransformStamped

// TransformStamped로부터 KDL::Frame 추출
auto transform_stamped = tf_buffer->lookupTransform(
  "base_link", "tool0", time);

KDL::Frame kdl_transform;
tf2::fromMsg(transform_stamped.transform, kdl_transform);

// KDL::Frame을 TransformStamped로 변환
geometry_msgs::msg::TransformStamped ts;
ts.header.stamp = node->now();
ts.header.frame_id = "base_link";
ts.child_frame_id = "tool0";
ts.transform = tf2::kdlToTransform(kdl_transform).transform;

5. TF2 변환 파이프라인과의 통합

5.1 KDL::Frame의 doTransform()

tf2_kdl은 KDL 타입에 대한 doTransform() 특수화를 제공하여 TF2 파이프라인에서 직접 사용할 수 있다.

// KDL::Frame 기반의 Stamped 타입 변환
tf2::Stamped<KDL::Frame> frame_in;
frame_in.frame_id_ = "camera_link";
frame_in.stamp_ = tf2::TimePoint(
  std::chrono::nanoseconds(node->now().nanoseconds()));
frame_in.setData(kdl_frame);

tf2::Stamped<KDL::Frame> frame_out;
auto transform = tf_buffer->lookupTransform(
  "base_link", "camera_link", time);
tf2::doTransform(frame_in, frame_out, transform);

6. 기구학 계산과의 통합

6.1 순기구학 결과의 TF2 발행

KDL의 기구학 솔버로 계산한 프레임을 TF2를 통해 발행하는 패턴이다.

#include <kdl/chainfksolverpos_recursive.hpp>

// 순기구학 솔버
KDL::ChainFkSolverPos_recursive fk_solver(kinematic_chain);

// 관절 각도 설정
KDL::JntArray joint_positions(n_joints);
// ... 관절 각도 설정

// 순기구학 계산
KDL::Frame end_effector_frame;
fk_solver.JntToCart(joint_positions, end_effector_frame);

// TF2로 발행
geometry_msgs::msg::TransformStamped ts;
ts.header.stamp = node->now();
ts.header.frame_id = "base_link";
ts.child_frame_id = "end_effector";
ts.transform = tf2::kdlToTransform(end_effector_frame).transform;
tf_broadcaster->sendTransform(ts);

6.2 TF2 변환을 역기구학 입력으로 활용

TF2에서 조회한 목표 자세를 KDL 역기구학 솔버의 입력으로 활용하는 패턴이다.

#include <kdl/chainiksolverpos_nr.hpp>

// TF2에서 목표 자세 조회
auto goal_transform = tf_buffer->lookupTransform(
  "base_link", "target_object", time);

// KDL::Frame으로 변환
KDL::Frame goal_frame;
tf2::fromMsg(goal_transform.transform, goal_frame);

// 역기구학 해석
KDL::JntArray q_init(n_joints);  // 초기 관절 각도
KDL::JntArray q_out(n_joints);   // 결과 관절 각도
int result = ik_solver.CartToJnt(q_init, goal_frame, q_out);

7. Eigen과의 비교

KDL은 로봇 기구학과 동역학에 특화된 기능을 제공하며, Eigen은 범용 수치 연산에 더 적합하다. 기구학 체인(kinematic chain) 기반의 연산에서는 KDL이, 대규모 행렬 연산이나 포인트 클라우드 처리에서는 Eigen이 주로 사용된다. MoveIt 2는 내부적으로 KDL과 Eigen을 모두 활용한다.

8. 주의사항

8.1 쿼터니언 순서

KDL의 Rotation::Quaternion(x, y, z, w) 함수는 (x, y, z, w) 순서를 사용한다. 이는 Eigen의 Quaterniond(w, x, y, z) 순서와 다르므로, 두 라이브러리 간 수동 변환 시 주의가 필요하다. tf2_kdl의 변환 함수는 이 순서 차이를 자동으로 처리한다.

8.2 KDL::Frame의 역변환

KDL::Frame::Inverse()는 SE(3)의 구조적 역변환을 수행하므로 효율적이다. 일반 행렬 역변환보다 계산 비용이 낮다.

8.3 메모리 할당

KDL의 기하학적 타입(Vector, Rotation, Frame)은 스택 할당(stack allocation)이 가능한 고정 크기 객체이므로, 동적 메모리 할당(heap allocation)에 의한 오버헤드가 없다.

9. 참고 문헌

• Orocos Project, “KDL: Kinematics and Dynamics Library,” https://www.orocos.org/kdl
• R. Smits, “KDL: Kinematics and Dynamics Library,” Orocos Documentation, https://docs.orocos.org/kdl/latest/
• Open Robotics, “tf2_kdl,” ROS 2 Documentation, https://docs.ros.org/en/rolling/p/tf2_kdl/

버전: 2026-03-26