Jogging và servo control: jog khớp và twist TCP

Bạn đã biết cách lên kế hoạch quỹ đạo với MoveJ và MoveL. Nhưng đôi khi bạn không cần một bản kế hoạch hoàn hảo — bạn chỉ cần điều khiển robot ngay lập tức, cử động một chút, xem phản ứng, rồi điều chỉnh tiếp. Đó chính là jogging và servo control.

Hãy tưởng tượng bạn đang dạy một học sinh múa. Cách đầu tiên: vạch ra toàn bộ bài múa trước rồi mới bắt đầu. Cách thứ hai: nắm tay học sinh và dẫn dắt từng động tác theo thời gian thực, điều chỉnh khi cần. MoveIt Servo chính là cách thứ hai.

MoveIt Servo là gì?

MoveIt Servo là package trong hệ sinh thái MoveIt 2 (ROS 2), được thiết kế cho điều khiển thời gian thực (real-time jogging/teleoperation). Thay vì lên kế hoạch đường đi rồi thực thi, Servo nhận lệnh liên tục từ người dùng (hoặc thuật toán) và chuyển đổi chúng thành lệnh joint ngay lập tức — thường ở tần số 100 Hz.

Điểm khác biệt then chốt:

	MoveIt Planning	MoveIt Servo
Mục đích	Tính đường đi tối ưu	Điều khiển real-time
Độ trễ	Vài giây (planning)	<10 ms
Input	Start + Goal pose	Luồng lệnh velocity/pose liên tục
Safety	Collision-free path	Scaling velocity khi nguy hiểm
Dùng khi	Automation chính xác	Teleop, teaching, visual servoing

MoveIt Servo chấp nhận 3 loại lệnh đầu vào:

JointJog — velocity của từng khớp riêng lẻ
TwistStamped — velocity tuyến tính + góc của TCP (tool center point)
PoseStamped — pose target cho TCP (pose tracking)

Kiến trúc bên trong: Inverse Jacobian

Câu hỏi cốt lõi: nếu người dùng muốn TCP di chuyển theo hướng X với tốc độ 0.1 m/s, thì mỗi khớp phải quay với tốc độ bao nhiêu?

Đây chính là bài toán inverse velocity kinematics, giải bằng ma trận Jacobian:

v_tcp = J(q) × dq/dt

Trong đó:
- v_tcp = [vx, vy, vz, ωx, ωy, ωz]ᵀ  (6D twist của TCP)
- J(q) = ma trận Jacobian 6×N (N = số DOF, thường là 6)
- dq/dt = [dq₁/dt, ..., dqN/dt]ᵀ (joint velocities)

Để đảo ngược: dq/dt = J⁺(q) × v_command

Thực tế, MoveIt Servo dùng damped least-squares (Levenberg-Marquardt pseudo-inverse) để tránh vấn đề chia cho 0 khi gần singularity:

dq/dt = Jᵀ × (J×Jᵀ + λ²I)⁻¹ × v_command

Hệ số damping λ tăng lên khi robot tiến gần singularity — làm giảm tốc độ trơn tru thay vì đột ngột dừng lại.

Servo cập nhật Jacobian mỗi chu kỳ control (100 Hz), nên luôn dùng kinematics hiện tại của robot.

Cài đặt MoveIt Servo

Cấu trúc package

Trước tiên tạo ROS 2 package cho servo controller:

cd ~/ros2_ws/src
ros2 pkg create my_servo_demo \
  --build-type ament_cmake \
  --dependencies rclcpp moveit_servo geometry_msgs sensor_msgs

CMakeLists.txt

cmake_minimum_required(VERSION 3.16)
project(my_servo_demo)

set(CMAKE_CXX_STANDARD 17)

# Tìm dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_servo REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(control_msgs REQUIRED)
find_package(std_srvs REQUIRED)

# Library chứa logic servo
add_library(servo_controller
  src/servo_controller.cpp
)
ament_target_dependencies(servo_controller
  rclcpp
  moveit_servo
  moveit_ros_planning
  geometry_msgs
  sensor_msgs
)
target_include_directories(servo_controller PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
)

# Executable: C++ servo node
add_executable(servo_demo_node src/servo_demo_node.cpp)
target_link_libraries(servo_demo_node servo_controller)
ament_target_dependencies(servo_demo_node
  rclcpp
  moveit_servo
  moveit_ros_planning_interface
  std_srvs
)

# Install targets
install(TARGETS servo_controller servo_demo_node
  DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY launch config
  DESTINATION share/${PROJECT_NAME}
)

ament_package()

package.xml

<?xml version="1.0"?>
<package format="3">
  <name>my_servo_demo</name>
  <version>0.1.0</version>
  <description>MoveIt Servo demo package</description>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>moveit_servo</depend>
  <depend>moveit_ros_planning</depend>
  <depend>moveit_ros_planning_interface</depend>
  <depend>geometry_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>control_msgs</depend>
  <depend>std_srvs</depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

Cấu hình servo_params.yaml

File config là trái tim của MoveIt Servo. Tạo config/servo_params.yaml:

moveit_servo:
  # --- Các topic input ---
  # Topic nhận lệnh velocity khớp (JointJog)
  joint_command_in_topic: ~/delta_joint_cmds

  # Topic nhận Cartesian twist (TwistStamped)
  cartesian_command_in_topic: ~/delta_twist_cmds

  # Topic nhận target pose (PoseStamped) — cho pose tracking
  pose_command_in_topic: ~/target_pose

  # --- Output ---
  # Frame gốc của robot
  planning_frame: base_link

  # End-effector link
  ee_frame_name: tool0

  # --- Safety ---
  # Ngưỡng singularity: condition number của Jacobian
  # Nhỏ hơn = nhạy hơn với singularity
  lower_singularity_threshold: 17.0
  hard_stop_singularity_threshold: 30.0

  # Kiểm tra va chạm
  check_collisions: true
  collision_check_rate: 10.0   # Hz (nên thấp hơn servo rate)
  self_collision_proximity_threshold: 0.01  # m
  scene_collision_proximity_threshold: 0.02 # m

  # Giới hạn khớp
  joint_limit_margin: 0.1  # radians — dừng trước hard limit

  # --- Tốc độ và gia tốc tối đa ---
  max_cartesian_speed: 2.0       # m/s cho TCP
  max_joint_velocity_override: 2.0  # rad/s per joint

  # --- Tần số servo loop ---
  publish_period: 0.005  # giây = 200 Hz publish rate

  # --- Smoothing ---
  use_smoothing: true
  smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"

  # --- Command timeout ---
  incoming_command_timeout: 0.1  # dừng nếu không nhận lệnh mới sau 0.1s

Launch file

Tạo launch/servo_demo.launch.py:

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
    # Load MoveIt config cho robot (thay "my_robot" bằng tên robot của bạn)
    moveit_config = (
        MoveItConfigsBuilder("my_robot", package_name="my_robot_moveit_config")
        .robot_description(file_path="config/my_robot.urdf.xacro")
        .robot_description_semantic(file_path="config/my_robot.srdf")
        .robot_description_kinematics(file_path="config/kinematics.yaml")
        .to_moveit_configs()
    )

    # Load servo params
    servo_params_file = os.path.join(
        get_package_share_directory("my_servo_demo"),
        "config",
        "servo_params.yaml",
    )

    # Servo Node
    servo_node = Node(
        package="moveit_servo",
        executable="servo_node",
        name="servo_node",
        output="screen",
        parameters=[
            servo_params_file,
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
        ],
        remappings=[
            # Map servo output sang controller của robot
            ("/servo_node/delta_joint_cmds", "/joint_group_velocity_controller/commands"),
        ],
    )

    # Move Group Node (cần cho planning scene và collision checking)
    move_group_node = Node(
        package="moveit_ros_move_group",
        executable="move_group",
        output="screen",
        parameters=[
            moveit_config.to_dict(),
            {"use_sim_time": True},
        ],
    )

    return LaunchDescription([
        move_group_node,
        servo_node,
    ])

C++ API: Cách dùng trực tiếp

MoveIt Servo cung cấp cả ROS interface (qua ServoNode) và C++ interface trực tiếp. C++ API cho phép tích hợp chặt hơn mà không cần overhead của ROS messaging.

Khởi tạo Servo instance

// src/servo_controller.cpp
#include <moveit_servo/servo.hpp>
#include <moveit_servo/utils/common.hpp>
#include <rclcpp/rclcpp.hpp>

using namespace moveit_servo;

class ServoController {
public:
    explicit ServoController(rclcpp::Node::SharedPtr node)
        : node_(node)
    {
        // Load servo parameters
        const std::string param_namespace = "moveit_servo";
        const auto param_listener =
            std::make_shared<const servo::ParamListener>(node_, param_namespace);
        const servo::Params servo_params = param_listener->get_params();

        // Tạo Planning Scene Monitor
        planning_scene_monitor_ =
            createPlanningSceneMonitor(node_, servo_params);

        // Tạo Servo instance
        servo_ = std::make_unique<Servo>(
            node_,
            param_listener,
            planning_scene_monitor_
        );

        RCLCPP_INFO(node_->get_logger(), "ServoController initialized");
    }

    // Jog từng khớp — ví dụ: quay khớp 1 với 0.1 rad/s
    void jogJoint(const std::string& joint_name, double velocity)
    {
        JointJogCommand cmd;
        cmd.joint_names = {joint_name};
        cmd.velocities = {velocity};

        servo_->setCommandType(CommandType::JOINT_JOG);
        KinematicState next_state = servo_->getNextJointState(cmd);

        // Dùng next_state để gửi lệnh cho hardware controller
        publishJointState(next_state);
    }

    // Jog theo Cartesian twist — di chuyển TCP theo frame được chỉ định
    void jogTwist(const std::string& frame_id,
                  double vx, double vy, double vz,
                  double wx, double wy, double wz)
    {
        TwistCommand cmd;
        cmd.frame_id = frame_id;
        cmd.velocities = {vx, vy, vz, wx, wy, wz};

        servo_->setCommandType(CommandType::TWIST);
        KinematicState next_state = servo_->getNextJointState(cmd);

        publishJointState(next_state);
    }

    // Pose tracking — di chuyển TCP đến target pose
    void trackPose(const std::string& frame_id,
                   const Eigen::Isometry3d& target_pose)
    {
        PoseCommand cmd;
        cmd.frame_id = frame_id;
        cmd.pose = target_pose;

        servo_->setCommandType(CommandType::POSE);
        KinematicState next_state = servo_->getNextJointState(cmd);

        publishJointState(next_state);
    }

    // Kiểm tra trạng thái an toàn
    StatusCode getStatus() const
    {
        return servo_->getStatus();
    }

private:
    void publishJointState(const KinematicState& state)
    {
        // TODO: Publish state sang hardware controller
        // Ví dụ: gửi trajectory_msgs/JointTrajectory
        // hoặc std_msgs/Float64MultiArray tùy loại controller
        
        if (servo_->getStatus() == StatusCode::NO_WARNING) {
            RCLCPP_DEBUG(node_->get_logger(),
                "Joint positions: %s", stateToString(state).c_str());
        }
    }

    rclcpp::Node::SharedPtr node_;
    planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
    std::unique_ptr<Servo> servo_;
};

Main control loop

// src/servo_demo_node.cpp
#include "my_servo_demo/servo_controller.hpp"
#include <rclcpp/rclcpp.hpp>
#include <chrono>

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<rclcpp::Node>("servo_demo");
    auto controller = std::make_shared<ServoController>(node);

    // Control loop ở 100 Hz
    rclcpp::Rate rate(100);
    int cycle = 0;

    RCLCPP_INFO(node->get_logger(), "Starting servo demo loop...");

    while (rclcpp::ok()) {
        // Ví dụ: jog TCP theo trục X trong 2 giây, rồi dừng
        if (cycle < 200) {
            // Di chuyển tới: vx = 0.05 m/s trong base_link frame
            controller->jogTwist("base_link",
                0.05, 0.0, 0.0,   // linear velocity (m/s)
                0.0,  0.0, 0.0);  // angular velocity (rad/s)
        } else if (cycle < 400) {
            // Di chuyển lui
            controller->jogTwist("base_link",
                -0.05, 0.0, 0.0,
                0.0,   0.0, 0.0);
        } else {
            // Dừng
            controller->jogTwist("base_link",
                0.0, 0.0, 0.0,
                0.0, 0.0, 0.0);
            cycle = 0;
        }

        // Kiểm tra trạng thái an toàn
        auto status = controller->getStatus();
        if (status == StatusCode::HALT_FOR_COLLISION) {
            RCLCPP_WARN(node->get_logger(), "Collision detected! Stopping.");
        } else if (status == StatusCode::HALT_FOR_SINGULARITY) {
            RCLCPP_WARN(node->get_logger(), "Near singularity! Slowing down.");
        }

        ++cycle;
        rclcpp::spin_some(node);
        rate.sleep();
    }

    rclcpp::shutdown();
    return 0;
}

Build với colcon

cd ~/ros2_ws
colcon build --packages-select my_servo_demo --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash

Joint Jog: Điều khiển từng khớp

Joint jog cho phép bạn quay từng khớp độc lập — hữu ích khi cần chỉnh một khớp cụ thể mà không muốn TCP di chuyển theo hướng phức tạp.

Qua ROS topic (dễ test)

# Terminal 1: Launch servo node
ros2 launch my_servo_demo servo_demo.launch.py

# Terminal 2: Publish JointJog message
ros2 topic pub --rate 10 /servo_node/delta_joint_cmds \
  control_msgs/msg/JointJog "
header:
  stamp:
    sec: 0
    nanosec: 0
  frame_id: base_link
joint_names: ['joint1']
velocities: [0.2]
duration: 0.0
"

Qua C++ API (hiệu quả hơn)

// Jog joint1 với 0.1 rad/s
JointJogCommand cmd;
cmd.joint_names = {"joint1", "joint2"};
cmd.velocities = {0.1, -0.05};  // rad/s

servo_->setCommandType(CommandType::JOINT_JOG);
KinematicState result = servo_->getNextJointState(cmd);

Lưu ý: joint_names và velocities phải có cùng số phần tử. Các khớp không được liệt kê sẽ không di chuyển.

Cartesian Twist Jog: Điều khiển TCP

Đây là mode phổ biến nhất trong thực tế. Người dùng chỉ định velocity của TCP theo 6 chiều (3 linear + 3 angular), Servo tự tính toán joint velocities tương ứng.

Frame awareness — điều quan trọng nhất

Khi gửi TwistCommand, bạn phải chỉ định frame_id vì nó quyết định hướng di chuyển:

// Trường hợp 1: Twist trong BASE frame
// vx = 0.1 m/s nghĩa là di chuyển theo hướng X của base_link
TwistCommand cmd_base;
cmd_base.frame_id = "base_link";
cmd_base.velocities = {0.1, 0.0, 0.0, 0.0, 0.0, 0.0};

// Trường hợp 2: Twist trong TOOL frame
// vx = 0.1 m/s nghĩa là di chuyển theo hướng "tới trước" của tool
TwistCommand cmd_tool;
cmd_tool.frame_id = "tool0";
cmd_tool.velocities = {0.1, 0.0, 0.0, 0.0, 0.0, 0.0};

Frame tool thường dùng hơn trong thực tế vì người vận hành muốn TCP đi "thẳng vào vật thể" chứ không quan tâm đến hướng trong base frame.

Lệnh angular velocity [ωx, ωy, ωz]:
- ωz = 0.5 rad/s trong tool0 → xoay tool quanh trục Z của nó
- ωz = 0.5 rad/s trong base_link → xoay tool quanh trục Z thế giới

An toàn: Singularity, Collision và Joint Limits

1. Singularity Detection

Singularity xảy ra khi robot mất 1 bậc tự do (joints thẳng hàng). Jacobian trở nên ill-conditioned, nghĩa là một velocity nhỏ của TCP đòi hỏi velocity rất lớn của khớp — nguy hiểm cho cơ học robot.

MoveIt Servo monitor condition number của Jacobian:

condition_number = σ_max / σ_min
(σ là singular values của Jacobian)

condition_number < lower_singularity_threshold → bình thường
condition_number giữa 2 threshold → giảm tốc độ (scaling)
condition_number > hard_stop_singularity_threshold → dừng ngay

Trong servo_params.yaml:

lower_singularity_threshold: 17.0  # bắt đầu giảm tốc
hard_stop_singularity_threshold: 30.0  # dừng hoàn toàn

2. Collision Checking

Servo tích hợp với MoveIt Planning Scene để kiểm tra va chạm liên tục:

check_collisions: true
collision_check_rate: 10.0  # Hz (lower = less CPU usage)

# Khoảng cách an toàn với vật thể (m)
scene_collision_proximity_threshold: 0.02

# Khoảng cách an toàn self-collision (m)
self_collision_proximity_threshold: 0.01

Khi robot tiến gần obstacle, velocity sẽ được scale down tỷ lệ với khoảng cách còn lại — không phải dừng đột ngột mà giảm dần.

3. Joint Limit Enforcement

joint_limit_margin: 0.1  # radians trước hard limit

Khi khớp cách hard limit ≤ joint_limit_margin, velocity theo hướng đó bị giảm xuống 0. Điều này ngăn robot vượt giới hạn cơ học.

Status codes

StatusCode status = servo_->getStatus();

switch (status) {
    case StatusCode::NO_WARNING:
        // Robot đang hoạt động bình thường
        break;
    case StatusCode::WARNING:
        // Tiếp tục nhưng có cảnh báo (gần singularity/limit)
        break;
    case StatusCode::HALT_FOR_SINGULARITY:
        // Robot đã dừng vì gần singularity
        RCLCPP_WARN(node->get_logger(), "Near singularity!");
        break;
    case StatusCode::HALT_FOR_COLLISION:
        // Robot đã dừng vì collision imminent
        RCLCPP_ERROR(node->get_logger(), "Collision detected!");
        break;
    case StatusCode::JOINT_BOUND:
        // Robot đã dừng vì joint limit
        RCLCPP_WARN(node->get_logger(), "Joint limit reached!");
        break;
}

Python: Script thử nghiệm nhanh

Trước khi viết C++ hoàn chỉnh, Python giúp bạn prototype rất nhanh. Đây là script keyboard teleop để jog robot qua Cartesian twist:

#!/usr/bin/env python3
"""
Keyboard teleop cho MoveIt Servo
Dùng để prototype hành vi jog trước khi hoàn thiện C++.

Phím:
  W/S  — TCP tới/lui (trục X trong tool frame)
  A/D  — TCP trái/phải (trục Y)
  Q/E  — TCP lên/xuống (trục Z)
  J/L  — Xoay quanh Z (yaw)
  I/K  — Xoay quanh Y (pitch)
  ESC  — Thoát
"""

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TwistStamped
import sys
import threading

# Cần install: pip install pynput
try:
    from pynput import keyboard
    HAS_PYNPUT = True
except ImportError:
    HAS_PYNPUT = False
    print("Cài pynput: pip install pynput")

class KeyboardTeleopNode(Node):
    """Node publish TwistStamped dựa trên input bàn phím."""

    LINEAR_SPEED = 0.05   # m/s — tốc độ an toàn cho beginner
    ANGULAR_SPEED = 0.3   # rad/s

    # Map phím → (axis_index, sign)
    KEY_MAP = {
        'w': (0, +1.0),  # linear X +
        's': (0, -1.0),  # linear X -
        'a': (1, +1.0),  # linear Y +
        'd': (1, -1.0),  # linear Y -
        'q': (2, +1.0),  # linear Z +
        'e': (2, -1.0),  # linear Z -
        'j': (5, +1.0),  # angular Z +  (yaw)
        'l': (5, -1.0),  # angular Z -
        'i': (4, +1.0),  # angular Y +  (pitch)
        'k': (4, -1.0),  # angular Y -
    }

    def __init__(self):
        super().__init__('keyboard_teleop')

        self.twist_pub = self.create_publisher(
            TwistStamped,
            '/servo_node/delta_twist_cmds',
            10
        )

        # State: 6D velocity vector [vx,vy,vz,wx,wy,wz]
        self.velocity = [0.0] * 6
        self._lock = threading.Lock()

        # Publish ở 50 Hz
        self.timer = self.create_timer(0.02, self._publish_twist)

        self.get_logger().info(
            "Keyboard teleop started. W/A/S/D/Q/E = linear, J/L/I/K = rotation"
        )

    def on_key_press(self, key):
        try:
            char = key.char.lower() if hasattr(key, 'char') else None
            if char in self.KEY_MAP:
                axis, sign = self.KEY_MAP[char]
                with self._lock:
                    speed = (self.LINEAR_SPEED if axis < 3
                             else self.ANGULAR_SPEED)
                    self.velocity[axis] = sign * speed
        except Exception:
            pass

    def on_key_release(self, key):
        # ESC để thoát
        if key == keyboard.Key.esc:
            rclpy.shutdown()
            return False

        try:
            char = key.char.lower() if hasattr(key, 'char') else None
            if char in self.KEY_MAP:
                axis, _ = self.KEY_MAP[char]
                with self._lock:
                    self.velocity[axis] = 0.0
        except Exception:
            pass

    def _publish_twist(self):
        msg = TwistStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'tool0'  # Jog trong tool frame

        with self._lock:
            v = self.velocity.copy()

        msg.twist.linear.x = v[0]
        msg.twist.linear.y = v[1]
        msg.twist.linear.z = v[2]
        msg.twist.angular.x = v[3]
        msg.twist.angular.y = v[4]
        msg.twist.angular.z = v[5]

        self.twist_pub.publish(msg)

def main():
    rclpy.init()
    node = KeyboardTeleopNode()

    if not HAS_PYNPUT:
        print("Cần pynput để nhận input bàn phím.")
        rclpy.shutdown()
        return

    # Chạy keyboard listener trên thread riêng
    listener = keyboard.Listener(
        on_press=node.on_key_press,
        on_release=node.on_key_release,
    )
    listener.start()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        listener.stop()
        node.destroy_node()
        if rclpy.ok():
            rclpy.shutdown()

if __name__ == '__main__':
    main()

Chạy thử

# Terminal 1: Ensure servo node đang chạy
ros2 launch my_servo_demo servo_demo.launch.py

# Terminal 2: Chạy keyboard teleop
python3 scripts/keyboard_teleop.py

Tại sao dùng Python để prototype?

Viết nhanh, không cần build
Dễ thử nhiều speed/frame khác nhau
Debug logic bàn phím → khi ổn mới chuyển sang C++
C++ thực hiện publishing giống hệt — chỉ là performance tốt hơn ở control loop tần số cao

Demo video

MoveIt Servo demo: real-time jogging với gamepad controller — nguồn: moveit2_tutorials

Pose Tracking: Theo dõi target pose thời gian thực

Ngoài velocity commands, Servo còn hỗ trợ pose tracking — robot tự động di chuyển TCP về một target pose được cập nhật liên tục (ví dụ: theo dõi vật thể qua camera).

# Python script publish target pose liên tục
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
import math

class PoseTracker(Node):
    def __init__(self):
        super().__init__('pose_tracker')
        self.pub = self.create_publisher(
            PoseStamped, '/servo_node/target_pose', 10
        )
        self.t = 0.0
        self.create_timer(0.02, self.publish_pose)  # 50 Hz

    def publish_pose(self):
        """Di chuyển TCP theo vòng tròn — demo pose tracking."""
        msg = PoseStamped()
        msg.header.frame_id = 'base_link'
        msg.header.stamp = self.get_clock().now().to_msg()

        # TCP vẽ vòng tròn r=0.1m trong mặt phẳng X-Y
        msg.pose.position.x = 0.5 + 0.1 * math.cos(self.t)
        msg.pose.position.y = 0.0 + 0.1 * math.sin(self.t)
        msg.pose.position.z = 0.3

        # Giữ orientation thẳng đứng
        msg.pose.orientation.x = 0.0
        msg.pose.orientation.y = 0.707
        msg.pose.orientation.z = 0.0
        msg.pose.orientation.w = 0.707

        self.pub.publish(msg)
        self.t += 0.1  # tốc độ vòng

def main():
    rclpy.init()
    node = PoseTracker()
    rclpy.spin(node)
    rclpy.shutdown()

C++ tương đương:

// Trong servo controller, set mode POSE
PoseCommand cmd;
cmd.frame_id = "base_link";

// Target pose được cập nhật từ vision system, AR marker, v.v.
cmd.pose = Eigen::Isometry3d::Identity();
cmd.pose.translation() = Eigen::Vector3d(0.5, 0.0, 0.3);

servo_->setCommandType(CommandType::POSE);
KinematicState result = servo_->getNextJointState(cmd);

Ứng dụng thực tế: Visual servoing — camera phát hiện marker → publish pose liên tục → Servo đưa TCP tới marker dù nó đang di chuyển.

Teleop với Xbox Controller

Để thực tế hơn trong lab, bạn có thể dùng Xbox controller thay vì bàn phím:

Xbox controller cho MoveIt teleoperation — nguồn: moveit2_tutorials

# Cài joy package
sudo apt install ros-humble-joy ros-humble-joy-linux

# Launch joy node
ros2 run joy joy_node

# Check topic
ros2 topic echo /joy

Map joystick axes sang TwistStamped:

Left stick X/Y → TCP linear X/Y
Right stick Y → TCP linear Z
Right stick X → TCP angular Z (yaw)
Bumpers → Joint jog cho khớp 1

Workflow thực tế: Từ prototype đến production

1. [Python] Viết keyboard_teleop.py → test concept với Servo node
      ↓ Xác nhận hành vi đúng
2. [Python] Thêm safety callbacks → monitor status codes
      ↓ Logic an toàn verified
3. [C++] Port sang C++ API → performance tốt hơn
      ↓ Test với hardware thật
4. [C++] Tích hợp với broader system → visual servoing, task planning

Khi nào nên dùng C++ thay Python:

Control loop >50 Hz (Python GIL giới hạn performance)
Tích hợp trực tiếp với hardware driver (giảm overhead)
Production system yêu cầu deterministic timing
Cần share Servo instance với các component C++ khác

Build và chạy toàn bộ

# Build workspace
cd ~/ros2_ws
colcon build --packages-select my_servo_demo --symlink-install
source install/setup.bash

# Launch (thay bằng robot simulator của bạn)
ros2 launch my_servo_demo servo_demo.launch.py

# Test joint jog từ terminal
ros2 topic pub --rate 10 /servo_node/delta_joint_cmds \
  control_msgs/msg/JointJog \
  "{joint_names: ['joint1'], velocities: [0.1]}"

# Test Cartesian jog
ros2 topic pub --rate 10 /servo_node/delta_twist_cmds \
  geometry_msgs/msg/TwistStamped \
  "{header: {frame_id: 'tool0'}, twist: {linear: {x: 0.05}}}"

Tóm tắt

Trong bài này bạn đã học:

MoveIt Servo là gì và tại sao khác với motion planning
Inverse Jacobian — toán học đằng sau Cartesian twist jogging
3 loại input: JointJog (per-joint), TwistStamped (Cartesian), PoseStamped (tracking)
CMakeLists.txt và package.xml đúng chuẩn cho ament_cmake
servo_params.yaml — cấu hình singularity, collision, joint limits
C++ API — khởi tạo Servo, set command type, lấy KinematicState
Python teleop — prototype nhanh trước khi viết C++
Frame awareness — tại sao frame_id quan trọng trong TwistCommand

Bài tiếp theo trong series sẽ đưa tất cả những gì đã học (FK, IK, MoveJ, MoveL, trajectory blending, profiles, và Servo) vào một motion planning pipeline hoàn chỉnh cho robot arm công nghiệp.