FK, IK và Jacobian: Toán học cánh tay robot

Vì sao bài này quan trọng?

Khi bạn gõ lệnh robot.moveTo(x=0.3, y=-0.2, z=0.5), robot không "teleport" đến đó. Phía sau là một chuỗi tính toán: Forward Kinematics (FK) để biết robot đang ở đâu, Inverse Kinematics (IK) để tìm góc khớp cần đặt, và Jacobian để biết liệu robot có thể di chuyển an toàn hay đang tiến đến vùng chết — singularity.

Đây là ba khái niệm nền tảng của mọi robot arm controller. Không hiểu chúng, bạn chỉ dùng robot như dùng tay cầm — nhấn nút và cầu trời. Hiểu chúng, bạn sẽ biết tại sao robot đột ngột rung lắc ở góc nhất định, tại sao IK trả về nghiệm "kỳ lạ", và tại sao không bao giờ cho robot đi qua wrist singularity với tốc độ cao.

Trong bài này, chúng ta sẽ:

1. Derive FK bằng tay dùng DH parameters cho UR5
2. Validate kết quả bằng ur_rtde Python API (getForwardKinematics, getInverseKinematics)
3. Điều khiển robot đến Cartesian target bằng moveJ_IK và moveL_FK
4. Tính Jacobian, reshape thành ma trận 6×6, và phát hiện singularity qua determinant

Roadmap series

Series Robot Arm Controller: Từ FK/IK đến ROS2 & UR RTDE gồm 5 bài:

1. DH Parameters — Ngôn ngữ chung của kinematics

4 tham số, vô số robot

Denavit-Hartenberg (DH) parameters là quy ước biểu diễn mỗi khớp robot bằng 4 con số:

Từ 4 tham số này, ta xây được một ma trận transformation 4×4 biểu diễn vị trí và hướng của frame khớp i so với frame khớp i-1:

T(i-1,i) = Rz(θ) · Tz(d) · Tx(a) · Rx(α)

         ┌ cos θ   -sin θ·cos α    sin θ·sin α    a·cos θ ┐
         │ sin θ    cos θ·cos α   -cos θ·sin α    a·sin θ │
       = │   0       sin α           cos α           d    │
         └   0         0               0             1    ┘

DH Parameters của UR5

Universal Robots công bố DH parameters chính thức cho toàn bộ dòng UR. Đây là bảng cho UR5 (đơn vị: mét và radian):

Vài điểm đáng chú ý:

• Khớp 1 có d₁ = 0.089159 m — chiều cao từ mặt phẳng gắn robot đến trục khớp 1
• Khớp 2, 3 có a ≠ 0 — đây là hai cánh tay dài của UR5 (cánh trên 425 mm, cánh dưới 392 mm)
• α = ±π/2 ở khớp 1, 4, 5 — tạo nên cấu trúc "khớp cổ tay hình cầu" (spherical wrist) ở 3 khớp cuối

2. Forward Kinematics — Từ góc khớp đến vị trí TCP

Implement bằng tay

FK là bài toán thuận: cho 6 góc khớp q = [θ₁, θ₂, θ₃, θ₄, θ₅, θ₆], tính vị trí và hướng của TCP (Tool Center Point) trong frame gốc (base frame).

Cách tính: nhân chuỗi 6 ma trận transformation:

T_base_to_TCP = T₀₁ · T₁₂ · T₂₃ · T₃₄ · T₄₅ · T₅₆

Đây là implementation Python với numpy:

import numpy as np

def dh_matrix(a: float, d: float, alpha: float, theta: float) -> np.ndarray:
    """Tính ma trận DH transformation 4x4."""
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)
    return np.array([
        [ct,  -st * ca,   st * sa,  a * ct],
        [st,   ct * ca,  -ct * sa,  a * st],
        [ 0,       sa,       ca,       d  ],
        [ 0,        0,        0,       1  ],
    ])

# DH parameters UR5: [a (m), d (m), alpha (rad)]
UR5_DH = [
    [0,        0.089159,  np.pi / 2],   # khớp 1
    [-0.425,   0,         0         ],  # khớp 2
    [-0.39225, 0,         0         ],  # khớp 3
    [0,        0.10915,   np.pi / 2],   # khớp 4
    [0,        0.09465,  -np.pi / 2],   # khớp 5
    [0,        0.0823,    0         ],  # khớp 6
]

def forward_kinematics(q: list, dh_params=UR5_DH) -> np.ndarray:
    """
    FK cho UR5 bằng DH parameters.
    q: 6 góc khớp [rad]
    Trả về: ma trận 4x4 pose của TCP trong base frame
    """
    T = np.eye(4)
    for i, (a, d, alpha) in enumerate(dh_params):
        T = T @ dh_matrix(a, d, alpha, q[i])
    return T

# Kiểm tra với cấu hình home của UR5
q_home = [0, -np.pi / 2, 0, -np.pi / 2, 0, 0]
T = forward_kinematics(q_home)

print("TCP Position (x, y, z):", T[:3, 3].round(4))
# → [0.0, -0.1093, 0.6508]  (xấp xỉ — phụ thuộc vào quy ước frame)

print("TCP Rotation matrix:")
print(T[:3, :3].round(4))

Validate bằng ur_rtde

Giờ so sánh kết quả tự tính với kết quả từ robot thật:

import rtde_control
import numpy as np

# Kết nối robot (UR5 tại IP 192.168.1.100)
rtde_c = rtde_control.RTDEControlInterface("192.168.1.100")

# Tính FK từ robot — trả về [x, y, z, rx, ry, rz]
# rx, ry, rz là rotation vector (axis-angle representation, đơn vị radian)
q_home = [0, -np.pi / 2, 0, -np.pi / 2, 0, 0]
pose_from_robot = rtde_c.getForwardKinematics(q_home)

print("FK từ ur_rtde:")
print(f"  Position: x={pose_from_robot[0]:.4f}, y={pose_from_robot[1]:.4f}, z={pose_from_robot[2]:.4f}")
print(f"  Rotation: rx={pose_from_robot[3]:.4f}, ry={pose_from_robot[4]:.4f}, rz={pose_from_robot[5]:.4f}")

# So sánh với kết quả manual
T_manual = forward_kinematics(q_home)
pos_manual = T_manual[:3, 3]
print("\nFK tự tính (DH parameters):")
print(f"  Position: x={pos_manual[0]:.4f}, y={pos_manual[1]:.4f}, z={pos_manual[2]:.4f}")

# Kết quả phải khớp trong sai số nhỏ (<1mm)
diff = np.linalg.norm(pos_manual - np.array(pose_from_robot[:3]))
print(f"\nSai lệch vị trí: {diff*1000:.2f} mm")

Lưu ý: ur_rtde dùng kinematic calibration từ robot thật (đã calibrate từ nhà máy), còn DH parameters ở trên là giá trị nominal. Sai lệch nhỏ (< 1-2 mm) là bình thường.

3. Inverse Kinematics — Bài toán ngược khó hơn nhiều

Tại sao IK khó hơn FK?

FK là bài toán có duy nhất một đáp án — một cấu hình khớp cho ra đúng một pose TCP. IK thì ngược lại: một pose TCP có thể có tới 16 nghiệm với robot 6-DOF dạng UR. Ngoài ra:

• Một số pose không tồn tại nghiệm (ngoài workspace)
• Các nghiệm khác nhau có thể gây robot đi theo đường rất khác nhau
• Cần chọn nghiệm gần cấu hình hiện tại nhất (qnear) để tránh vọt lố

Đây là lý do tham số qnear trong getInverseKinematics quan trọng: nó hướng solver về nghiệm gần nhất với cấu hình hiện tại, thay vì trả về nghiệm ngẫu nhiên.

Dùng ur_rtde API

import rtde_control
import numpy as np

rtde_c = rtde_control.RTDEControlInterface("192.168.1.100")

# Pose mục tiêu [x, y, z, rx, ry, rz] — đơn vị: m và rad
target_pose = [0.3, -0.3, 0.4, 0.0, np.pi, 0.0]

# Cấu hình hiện tại làm seed cho IK solver
q_current = [0, -np.pi / 2, 0, -np.pi / 2, 0, 0]

# Bước 1: Kiểm tra có nghiệm không — KHÔNG throw exception
has_solution = rtde_c.getInverseKinematicsHasSolution(
    target_pose,
    qnear=q_current,
    max_position_error=1e-6,
    max_orientation_error=1e-6,
)

if has_solution:
    # Bước 2: Lấy nghiệm — seed qnear để chọn nghiệm gần nhất
    q_solution = rtde_c.getInverseKinematics(
        target_pose,
        qnear=q_current,
    )
    print("IK solution [deg]:", [np.degrees(q) for q in q_solution])
else:
    print("Không có nghiệm IK — pose ngoài workspace hoặc gần singularity!")

Bẫy thường gặp: Gọi getInverseKinematics() mà không kiểm tra getInverseKinematicsHasSolution() trước có thể trả về mảng rỗng hoặc nghiệm không hợp lệ nếu không có solution. Luôn kiểm tra trước.

4. moveJ_IK và moveL_FK — Điều khiển thực tế

Có 4 lệnh motion chính trong ur_rtde liên quan đến kinematics:

moveJ_IK — Joint interpolation đến Cartesian target

import rtde_control
import numpy as np

rtde_c = rtde_control.RTDEControlInterface("192.168.1.100")

# Di chuyển đến pose Cartesian bằng joint interpolation
# Robot tự giải IK rồi dùng moveJ
target = [0.3, -0.3, 0.4, 0.0, np.pi, 0.0]  # [x, y, z, rx, ry, rz]

success = rtde_c.moveJ_IK(
    target,
    speed=1.05,        # joint speed [rad/s], default 1.05
    acceleration=1.4,  # joint acceleration [rad/s²], default 1.4
    asynchronous=False # True = không blocking, False = chờ đến đích
)
print("moveJ_IK success:", success)

Ưu điểm của moveJ_IK: Nhanh, robot chọn đường đi tối ưu trong joint space. Dùng khi cần đến điểm mới, không quan trọng đường đi.

moveL_FK — Linear Cartesian motion từ joint configuration

# Di chuyển theo đường thẳng Cartesian đến điểm xác định bởi FK của q
# Hữu ích khi bạn đã có cấu hình khớp muốn đến nhưng muốn đường đi thẳng
q_target = [0, -np.pi / 2, np.pi / 4, -3 * np.pi / 4, 0, 0]

success = rtde_c.moveL_FK(
    q_target,
    speed=0.25,        # TCP speed [m/s], default 0.25
    acceleration=1.2,  # TCP acceleration [m/s²], default 1.2
)
print("moveL_FK success:", success)

Ưu điểm của moveL_FK: Đường đi TCP là đường thẳng trong không gian Cartesian. Quan trọng khi cần tránh vật cản hoặc đảm bảo tiếp xúc vuông góc.

5. Jacobian và phát hiện Singularity

Jacobian là gì?

Jacobian (ký hiệu J) là ma trận ánh xạ vận tốc khớp sang vận tốc TCP:

ẋ = J(q) · q̇

Trong đó:
  ẋ = [vx, vy, vz, ωx, ωy, ωz]ᵀ  — vận tốc TCP (6 chiều)
  q̇ = [θ̇₁, θ̇₂, θ̇₃, θ̇₄, θ̇₅, θ̇₆]ᵀ — vận tốc khớp (6 chiều)
  J  là ma trận 6×6 phụ thuộc vào cấu hình q hiện tại

Hay theo chiều ngược lại: nếu muốn TCP di chuyển với vận tốc ẋ, cần joint velocity:

q̇ = J⁻¹(q) · ẋ

Singularity xảy ra khi J là ma trận suy biến — det(J) → 0. Lúc này, ma trận nghịch đảo J⁻¹ không tồn tại, và robot cần vận tốc khớp vô cực để di chuyển TCP theo một số hướng nhất định.

3 loại singularity trong UR robots

┌─────────────────────────────────────────────────────────────────┐
│  LOẠI SINGULARITY        │ NGUYÊN NHÂN        │ TRIỆU CHỨNG     │
│─────────────────────────────────────────────────────────────────│
│  Shoulder (vai)          │ Wrist center nằm   │ Khớp 1 quay     │
│                          │ trên trục khớp 1   │ không kiểm soát │
│─────────────────────────────────────────────────────────────────│
│  Elbow (khuỷu)           │ Khớp 3 = 0 hoặc π │ Cánh tay        │
│                          │ (arm fully extended│ co/duỗi hết cỡ  │
│                          │  hoặc fully folded)│                  │
│─────────────────────────────────────────────────────────────────│
│  Wrist (cổ tay)          │ Khớp 5 = 0 hoặc π │ Khớp 4 và 6     │
│  *** PHỔ BIẾN NHẤT ***   │ (axes 4 và 6 thẳng │ quay điên       │
│                          │  hàng)             │                  │
└─────────────────────────────────────────────────────────────────┘

Tính Jacobian và detect singularity

import rtde_control
import numpy as np

rtde_c = rtde_control.RTDEControlInterface("192.168.1.100")

def check_singularity(rtde_c, threshold: float = 1e-4) -> tuple[float, bool]:
    """
    Kiểm tra singularity dựa vào Jacobian determinant.
    Trả về (det_J, is_near_singularity).
    """
    q = rtde_c.getActualQ()  # lấy góc khớp hiện tại

    # getJacobian() trả về flat list 36 phần tử (row-major)
    J_flat = rtde_c.getJacobian(q)
    J = np.array(J_flat).reshape(6, 6)

    det_J = abs(np.linalg.det(J))
    return det_J, det_J < threshold

# Ví dụ 1: cấu hình bình thường
q_safe = [0, -np.pi / 2, np.pi / 4, -3 * np.pi / 4, 0, 0]
J_flat = rtde_c.getJacobian(q_safe)
J = np.array(J_flat).reshape(6, 6)
det_safe = abs(np.linalg.det(J))
print(f"Cấu hình an toàn — det(J) = {det_safe:.6f}")

# Ví dụ 2: gần wrist singularity (khớp 5 gần 0)
q_singular = [0, -np.pi / 2, 0, -np.pi / 2, 0.001, 0]  # θ₅ ≈ 0
J_flat_s = rtde_c.getJacobian(q_singular)
J_s = np.array(J_flat_s).reshape(6, 6)
det_sing = abs(np.linalg.det(J_s))
print(f"Gần wrist singularity — det(J) = {det_sing:.6f}")

# Dùng trong control loop
def safe_move_cartesian(rtde_c, target_pose: list, speed=0.1):
    """Di chuyển đến pose, dừng lại nếu gần singularity."""
    det_J, is_singular = check_singularity(rtde_c)

    if is_singular:
        print(f"DỪNG: Gần singularity! det(J) = {det_J:.2e}")
        print("Thử di chuyển theo hướng khác hoặc dùng jointspace.")
        return False

    rtde_c.moveL(target_pose, speed=speed)
    return True

Minimum Singular Value — Cách tốt hơn

Determinant nhạy cảm với scale, nên trong thực tế người ta hay dùng minimum singular value của Jacobian qua SVD:

def singularity_measure(J: np.ndarray) -> float:
    """
    Minimum singular value — measure tốt hơn det(J).
    Gần 0 = gần singularity.
    Giá trị < 0.01 là cảnh báo, < 0.001 là dừng.
    """
    _, singular_values, _ = np.linalg.svd(J)
    return singular_values[-1]  # giá trị nhỏ nhất

# Dùng trong control loop:
J_flat = rtde_c.getJacobian()
J = np.array(J_flat).reshape(6, 6)

sigma_min = singularity_measure(J)
if sigma_min < 0.01:
    print(f"WARNING: σ_min = {sigma_min:.4f} — gần singularity, giảm tốc!")
if sigma_min < 0.001:
    print(f"CRITICAL: σ_min = {sigma_min:.4f} — DỪNG ngay!")

6. Pipeline đầy đủ — Từ pose đến motion an toàn

Kết hợp tất cả lại thành một hàm safe_cartesian_move hoàn chỉnh:

import rtde_control
import rtde_receive
import numpy as np

class URController:
    SINGULARITY_WARN  = 0.01   # sigma_min threshold warning
    SINGULARITY_STOP  = 0.001  # sigma_min threshold stop

    def __init__(self, ip: str):
        self.rtde_c = rtde_control.RTDEControlInterface(ip)
        self.rtde_r = rtde_receive.RTDEReceiveInterface(ip)

    def get_jacobian(self) -> np.ndarray:
        q = self.rtde_r.getActualQ()
        J_flat = self.rtde_c.getJacobian(q)
        return np.array(J_flat).reshape(6, 6)

    def singularity_ok(self) -> bool:
        J = self.get_jacobian()
        _, sv, _ = np.linalg.svd(J)
        sigma_min = sv[-1]
        if sigma_min < self.SINGULARITY_STOP:
            print(f"DỪNG: σ_min={sigma_min:.4f}")
            return False
        if sigma_min < self.SINGULARITY_WARN:
            print(f"CẢNH BÁO: σ_min={sigma_min:.4f}")
        return True

    def move_to_pose(self, target: list, use_linear: bool = True) -> bool:
        """
        Di chuyển đến target pose với kiểm tra singularity.
        use_linear=True: moveL (đường thẳng Cartesian)
        use_linear=False: moveJ_IK (joint interpolation, nhanh hơn)
        """
        q_current = self.rtde_r.getActualQ()

        # Kiểm tra IK tồn tại
        if not self.rtde_c.getInverseKinematicsHasSolution(target, q_current):
            print("Không có nghiệm IK cho target pose này!")
            return False

        # Kiểm tra singularity tại cấu hình hiện tại
        if not self.singularity_ok():
            return False

        # Thực hiện motion
        if use_linear:
            return self.rtde_c.moveL(target, speed=0.1, acceleration=0.5)
        else:
            return self.rtde_c.moveJ_IK(target, speed=1.05, acceleration=1.4)

# Sử dụng:
# ur = URController("192.168.1.100")
# ur.move_to_pose([0.3, -0.3, 0.4, 0, np.pi, 0], use_linear=True)

7. Những điều cần nhớ

FK là tính thuận, duy nhất: Một cấu hình khớp → một pose TCP. Không có ambiguity.

IK là tính ngược, nhiều nghiệm: Luôn dùng qnear để seed solver về nghiệm gần nhất. Luôn kiểm tra getInverseKinematicsHasSolution() trước.

moveJ_IK vs moveL: dùng moveJ_IK khi tốc độ quan trọng, đường đi không quan trọng. Dùng moveL khi cần TCP đi thẳng (tiếp cận chi tiết, hàn theo đường).

Jacobian det → 0 là singularity: Monitor sigma_min của SVD, không chỉ determinant. Threshold thực tế: warning ở 0.01, stop ở 0.001.

Wrist singularity là phổ biến nhất: Xảy ra khi khớp 5 gần 0° hoặc 180°. Triệu chứng: khớp 4 và 6 quay điên với tốc độ rất cao. Tránh bằng cách: không để TCP đi qua pose "thẳng đứng" của wrist.

Kết luận

Bài này đã cover nền tảng kinematics: derive FK bằng DH parameters cho UR5, validate với ur_rtde API, dùng moveJ_IK/moveL_FK để điều khiển Cartesian, và detect singularity qua Jacobian SVD.

Bài tiếp theo sẽ đi sâu vào coordinate frames — tại sao robot cần phân biệt base frame, tool frame, và user frame, và làm thế nào jogStart() với setTcp() hoạt động trong từng frame.

Bài viết liên quan

• Bài 2: Frames, Jogging & TCP — Hệ tọa độ trong UR robot
• Bài 3: Motion Primitives — MoveJ, MoveL, MoveC và MoveP
• Bài 5: ROS2 & RTDE — Full stack robot arm controller
• G1 MuJoCo: Từ PD giữ dáng đến WBID có tiếp xúc
• Robot Manipulation: Velocity Control nâng cao