Mô hình cánh tay robot: joints, links, URDF, DH, SE(3)

Trong bài trước, ta đã dựng một project C++/CMake/Eigen đủ sạch để phát triển controller. Bài này đặt viên gạch kỹ thuật đầu tiên lên project đó: robot model. Nếu không có model đúng, mọi thứ phía sau đều sai theo cách rất khó debug. Forward kinematics sẽ trả pose lệch vài centimet, inverse kinematics hội tụ tới nghiệm lạ, MoveL nhìn thẳng trong phần mềm nhưng robot thật lại cong, còn jogging theo trục tool có thể chạy ngược hướng.

Robot model cho tay máy nối tiếp có hai lớp ý nghĩa. Lớp hình học mô tả khớp, link, trục quay, chiều dài, offset, frame base, frame flange và TCP. Lớp phần mềm mô tả cùng thông tin đó trong một cấu trúc mà code có thể tính toán: bảng DH, ma trận homogeneous transform, URDF, hoặc model đã parse bằng thư viện như Robotics Toolbox và Pinocchio. Mục tiêu của bài này là bạn tự điền được bảng DH cho UR5, tự viết được model nhỏ bằng Python/NumPy và C++/Eigen, rồi biết khi nào nên chuyển sang URDF và thư viện động học chuẩn.

Joint space và Cartesian space

Joint space là không gian của biến khớp. Với UR5, robot có 6 khớp quay nên một cấu hình robot là vector:

q = [q1, q2, q3, q4, q5, q6]^T

Mỗi qi thường tính bằng radian. Khi bạn gửi MoveJ, controller chủ yếu lập kế hoạch trong joint space: mỗi khớp đi từ góc hiện tại tới góc đích theo profile vận tốc/gia tốc. Ưu điểm là dễ kiểm soát giới hạn khớp, singularity ít gây bất ngờ hơn, và đường đi thường mượt với cơ cấu robot. Nhược điểm là TCP không đi theo đường thẳng trong không gian làm việc.

Cartesian space, hay task space trong nhiều tài liệu, là không gian pose của đầu công tác. Một pose 6D gồm vị trí (x, y, z) và orientation. Trong code cổ điển, pose thường được biểu diễn bằng một phần tử của SE(3): ma trận 4x4 chứa rotation 3x3 và translation 3x1. Khi bạn gửi MoveL, ý định là TCP đi theo đường thẳng trong Cartesian space, sau đó controller phải giải IK liên tục để biến mỗi pose trung gian thành q.

Hai không gian này liên hệ bằng forward kinematics và inverse kinematics:

q trong joint space  --FK-->  T_base_tool trong SE(3)
T_base_tool          --IK-->  q hoặc nhiều nghiệm q

Điểm beginner hay nhầm là "robot đang ở đâu" không chỉ là q, cũng không chỉ là (x, y, z). q nói robot gập khớp thế nào. Pose nói TCP đang nằm và xoay thế nào so với một frame tham chiếu. Một pose có thể có nhiều nghiệm q; ngược lại một q xác định đúng một pose nếu model đúng.

Links, joints, base, flange, TCP

Một tay máy nối tiếp gồm các link cứng nối với nhau bằng joint. Link là thân cơ khí: đế, upper arm, forearm, wrist. Joint là bậc tự do giữa hai link: revolute joint quay quanh một trục, prismatic joint tịnh tiến dọc một trục. UR5 có 6 revolute joints.

Trong model, mỗi joint có một trục. Với DH, trục z_i thường được gán trùng với trục chuyển động của joint. Với URDF, joint có parent link, child link, origin và axis; hình học mesh, inertia và collision cũng sống trong link/joint tree. Vì vậy URDF giàu thông tin hơn DH, nhưng DH gọn hơn cho bài toán FK của serial arm.

Các frame quan trọng:

• base_link: frame gốc của cây động học theo convention ROS. Với Universal Robots ROS 2 documentation, base_link là root link và tuân theo REP-103.
• base: frame controller dùng để hiển thị pose trên teach pendant. Với UR, base cùng vị trí với base_link nhưng khác orientation.
• flange: mặt bích cơ khí ở cuối cổ tay, nơi lắp gripper hoặc tool.
• tool0: tool frame khi TCP chưa cấu hình offset. Trong hệ UR ROS, tool0 tương ứng tool frame từ FK khi tool là all-zero.
• TCP: Tool Center Point thật của ứng dụng, ví dụ tâm hút chân không, đầu mũi hàn, hoặc tâm giữa hai ngón gripper. TCP thường là transform cố định từ flange hoặc tool0.

Khi viết controller, hãy luôn ghi rõ pose bạn đang dùng là T_base_tool0, T_base_flange hay T_base_tcp. Không nên đặt biến chung chung là pose trong lớp core kinematics. Một lỗi frame nhỏ có thể biến thành lỗi tool offset lớn ở robot thật.

SE(3) và homogeneous transform

Trong robotics, pose cứng 3D thường viết:

T = [ R  p ]
    [ 0  1 ]

R là ma trận rotation 3x3, p là vector translation 3x1. Tập hợp các transform hợp lệ như vậy gọi là SE(3), special Euclidean group. Điều kiện quan trọng là R phải trực chuẩn và det(R) = 1. Dòng cuối luôn là [0, 0, 0, 1].

Ma trận này làm ba việc cùng lúc:

1. Biểu diễn cấu hình của một frame so với frame khác, ví dụ T_base_tcp.
2. Đổi tọa độ điểm từ frame này sang frame khác.
3. Ghép chuỗi transform bằng phép nhân ma trận.

Nếu T_0_1 là pose của frame 1 trong frame 0, và T_1_2 là pose của frame 2 trong frame 1, thì:

T_0_2 = T_0_1 * T_1_2

Forward kinematics của serial arm chỉ là nhân chuỗi transform từ base tới tool:

T_base_tool = A1(q1) * A2(q2) * A3(q3) * A4(q4) * A5(q5) * A6(q6) * T_flange_tcp

Đây là lý do ta cần robot model. Nếu từng Ai(qi) đúng, FK đúng. Nếu một dấu alpha sai, kết quả xoay sai từ link đó trở đi.

DH là gì?

Denavit-Hartenberg parameters nén quan hệ giữa hai frame liên tiếp thành bốn số:

Với standard DH, transform thường viết:

A_i = RotZ(theta_i) * TransZ(d_i) * TransX(a_i) * RotX(alpha_i)

Ma trận tương ứng:

[ cosθ  -sinθ cosα   sinθ sinα   a cosθ ]
[ sinθ   cosθ cosα  -cosθ sinα   a sinθ ]
[ 0      sinα        cosα        d      ]
[ 0      0           0           1      ]

Modified DH, thường gắn với Craig convention, dùng thứ tự transform khác và vị trí frame khác. Một dạng phổ biến:

A_i = RotX(alpha_{i-1}) * TransX(a_{i-1}) * RotZ(theta_i) * TransZ(d_i)

Đừng trộn hai convention trong cùng project. Bảng số có thể nhìn gần giống nhau nhưng FK sẽ khác. Universal Robots công bố bảng a, d, alpha cho các model UR và lưu ý a của UR tương ứng với r trong nhiều mô tả DH. Trong bài này ta dùng standard DH cho phần from scratch vì ma trận dễ viết và dễ kiểm tra.

Điền bảng DH UR5 bằng tay

Bảng chính thức của Universal Robots cho UR5 classic dùng các giá trị hình học sau:

Vì cả 6 khớp đều là revolute, theta_i là biến khớp:

theta_i = q_i + theta_offset_i

Với mô hình tối giản, lấy offset bằng 0. Trong robot thật, calibration file có thể chứa correction riêng từng robot. Đừng dùng bảng generic để đòi chính xác tuyệt đối ở cấp milimet.

Quy trình điền bằng tay:

1. Vẽ skeleton robot ở tư thế zero. Không cần đẹp, chỉ cần thể hiện 6 trục quay. Với UR5, khớp 2 và 3 gần song song; cụm wrist có ba khớp cắt nhau gần cổ tay.
2. Gán z_i trùng với trục joint i. Với revolute joint, chiều dương theo quy tắc bàn tay phải.
3. Gán x_i là pháp tuyến chung từ z_i tới z_{i+1}. Nếu hai trục cắt nhau, a bằng 0 và x_i chọn theo convention để tạo hệ tay phải.
4. Gán y_i = z_i x x_i để hoàn thành frame tay phải.
5. Đo a_i: khoảng cách dọc x_i. Với UR5, link shoulder-elbow và elbow-wrist tạo hai giá trị âm -0.425 và -0.39225 theo convention UR.
6. Đo d_i: offset dọc z_i. Các offset lớn nằm ở base và wrist: 0.089159, 0.10915, 0.09465, 0.0823.
7. Đo alpha_i: góc xoay quanh x_i từ z_i sang z_{i+1}. UR5 có pi/2, 0, 0, pi/2, -pi/2, 0.
8. Ghi theta_i là biến. Nếu cần match một URDF cụ thể, thêm offset và kiểm tra FK ở nhiều cấu hình.

Sơ đồ frame ASCII để tự kiểm:

base
  z1 up through shoulder yaw
  |
  o---- x1 / common normal to joint 2
       shoulder pitch z2  === upper arm ===  elbow pitch z3
                                      === forearm === wrist1 z4
                                                       wrist2 z5
                                                       wrist3 z6 -> flange/tool0

Standard DH chain:
T_0_6 = A1(q1) A2(q2) A3(q3) A4(q4) A5(q5) A6(q6)

Một cách kiểm nhanh: nhập q = zeros(6) vào FK NumPy bên dưới, in T_0_6, rồi so với Robotics Toolbox hoặc Pinocchio URDF ở cùng convention base/tool. Nếu vị trí gần đúng nhưng orientation lệch 180 độ, có thể bạn đang so base_link với base, hoặc flange với tool0.

DH hay URDF?

DH phù hợp khi bạn cần:

• Giảng giải FK/IK từ đầu.
• Tính nhanh serial chain 6-DOF.
• Viết controller nhỏ không cần mesh, inertia, collision.
• Debug từng link transform bằng bảng số.

URDF phù hợp khi bạn cần:

• Tích hợp ROS 2, TF, RViz, MoveIt, Gazebo, Isaac Sim.
• Mô tả link tree có fixed frames, gripper, camera, force-torque sensor.
• Giữ joint limits, visual mesh, collision mesh, inertia.
• Load model bằng Pinocchio, KDL, MoveIt hoặc các thư viện dynamics.

Trong sản phẩm thật, tôi thường giữ cả hai mức: DH hoặc một model hình học nhỏ để unit test core FK, và URDF để tích hợp hệ sinh thái ROS/visualization/planning. Quan trọng là có test so sánh hai nguồn ở vài cấu hình chuẩn.

Python from scratch bằng NumPy

File Python tối giản:

import numpy as np

UR5_DH = [
    # a, alpha, d, theta_offset
    (0.0,      np.pi / 2, 0.089159, 0.0),
    (-0.425,   0.0,       0.0,      0.0),
    (-0.39225, 0.0,       0.0,      0.0),
    (0.0,      np.pi / 2, 0.10915,  0.0),
    (0.0,     -np.pi / 2, 0.09465,  0.0),
    (0.0,      0.0,       0.0823,   0.0),
]

def dh_standard(a: float, alpha: float, d: float, theta: float) -> np.ndarray:
    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.0, sa,       ca,      d],
        [0.0, 0.0,      0.0,     1.0],
    ], dtype=float)

def fk(dh_table, q: np.ndarray) -> np.ndarray:
    if len(dh_table) != len(q):
        raise ValueError("DH table and q must have the same length")

    T = np.eye(4)
    for (a, alpha, d, theta_offset), qi in zip(dh_table, q):
        T = T @ dh_standard(a, alpha, d, qi + theta_offset)
    return T

if __name__ == "__main__":
    q = np.zeros(6)
    T = fk(UR5_DH, q)
    np.set_printoptions(precision=6, suppress=True)
    print(T)

Điểm cần chú ý:

• Tất cả góc là radian.
• Bảng dùng thứ tự (a, alpha, d, theta_offset) để code gần với công thức ma trận.
• T = T @ A_i nghĩa là ta nhân transform từ trái sang phải trong cùng convention.
• Không dùng Euler angle ở core FK. Euler chỉ nên dùng ở UI hoặc log vì dễ nhầm thứ tự quay.

Sau khi có FK, bạn có thể thêm TCP offset:

def transl(x, y, z):
    T = np.eye(4)
    T[:3, 3] = [x, y, z]
    return T

T_flange_tcp = transl(0.0, 0.0, 0.120)
T_base_tcp = fk(UR5_DH, q) @ T_flange_tcp

Ở các bài sau về FK/IK/Jacobian, ta sẽ dùng chính model này để tính Jacobian số và kiểm tra IK.

C++ from scratch bằng Eigen

Trong C++, nên biểu diễn pose bằng Eigen::Isometry3d khi đó là rigid transform hợp lệ. Khi cần in hoặc trao đổi với API cũ, chuyển sang Eigen::Matrix4d. Isometry3d thể hiện rõ ý định: rotation + translation, không scale, không shear.

Ví dụ header model:

#pragma once

#include <Eigen/Geometry>
#include <string>
#include <vector>

namespace arm {

enum class JointType {
    Revolute,
    Prismatic,
    Fixed,
};

struct Joint {
    std::string name;
    JointType type{JointType::Revolute};
    double lower{-3.141592653589793};
    double upper{3.141592653589793};
};

struct Link {
    std::string name;
    double a{0.0};
    double alpha{0.0};
    double d{0.0};
    double theta_offset{0.0};
    Joint joint;
};

Eigen::Isometry3d dhStandard(double a, double alpha, double d, double theta);
std::vector<Link> parseDhCsv(const std::string& path);
Eigen::Isometry3d forwardKinematics(const std::vector<Link>& links, const Eigen::VectorXd& q);

}  // namespace arm

Implementation:

#include "arm/model.hpp"

#include <Eigen/Dense>
#include <fstream>
#include <sstream>
#include <stdexcept>

namespace arm {

Eigen::Isometry3d dhStandard(double a, double alpha, double d, double theta) {
    const double ct = std::cos(theta);
    const double st = std::sin(theta);
    const double ca = std::cos(alpha);
    const double sa = std::sin(alpha);

    Eigen::Matrix4d M;
    M << ct, -st * ca,  st * sa, a * ct,
         st,  ct * ca, -ct * sa, a * st,
         0.0, sa,       ca,      d,
         0.0, 0.0,      0.0,     1.0;

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    T.matrix() = M;
    return T;
}

std::vector<Link> parseDhCsv(const std::string& path) {
    std::ifstream file(path);
    if (!file) {
        throw std::runtime_error("Cannot open DH file: " + path);
    }

    std::vector<Link> links;
    std::string line;
    while (std::getline(file, line)) {
        if (line.empty() || line[0] == '#') {
            continue;
        }

        std::stringstream ss(line);
        std::string name, a, alpha, d, theta_offset;
        std::getline(ss, name, ',');
        std::getline(ss, a, ',');
        std::getline(ss, alpha, ',');
        std::getline(ss, d, ',');
        std::getline(ss, theta_offset, ',');

        Link link;
        link.name = name;
        link.a = std::stod(a);
        link.alpha = std::stod(alpha);
        link.d = std::stod(d);
        link.theta_offset = std::stod(theta_offset);
        link.joint.name = name + "_joint";
        links.push_back(link);
    }
    return links;
}

Eigen::Isometry3d forwardKinematics(const std::vector<Link>& links, const Eigen::VectorXd& q) {
    if (static_cast<int>(links.size()) != q.size()) {
        throw std::runtime_error("links.size() must match q.size()");
    }

    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    for (int i = 0; i < q.size(); ++i) {
        const Link& link = links[static_cast<std::size_t>(i)];
        T = T * dhStandard(link.a, link.alpha, link.d, q[i] + link.theta_offset);
    }
    return T;
}

}  // namespace arm

CSV cho UR5:

# name,a,alpha,d,theta_offset
shoulder_pan,0.0,1.5707963267948966,0.089159,0.0
shoulder_lift,-0.425,0.0,0.0,0.0
elbow,-0.39225,0.0,0.0,0.0
wrist_1,0.0,1.5707963267948966,0.10915,0.0
wrist_2,0.0,-1.5707963267948966,0.09465,0.0
wrist_3,0.0,0.0,0.0823,0.0

CMake tối giản nối Eigen:

cmake_minimum_required(VERSION 3.16)
project(arm_model_demo LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(Eigen3 REQUIRED)

add_library(arm_model src/model.cpp)
target_include_directories(arm_model PUBLIC include)
target_link_libraries(arm_model PUBLIC Eigen3::Eigen)

add_executable(demo src/main.cpp)
target_link_libraries(demo PRIVATE arm_model)

Lệnh build:

cmake -S . -B build -G Ninja
cmake --build build
./build/demo ur5_dh.csv

Trong main.cpp, in T.matrix() và so với Python. Nếu Python và C++ khác nhau, đừng chạy tiếp sang IK. Hãy sửa convention trước.

Dùng Robotics Toolbox for Python

Robotics Toolbox for Python của Peter Corke rất hữu ích để kiểm chứng. Nó có model DH, model URDF, SE3, quỹ đạo và Swift visualizer. Với UR5, cách dùng thường là:

import numpy as np
import roboticstoolbox as rtb

robot = rtb.models.DH.UR5()
q = np.zeros(6)
T = robot.fkine(q)
print(T)

robot.plot(q, backend="swift")

Nếu version bạn cài không có đúng class UR5, hãy liệt kê rtb.models.DH hoặc dùng DHRobot tự tạo từ các RevoluteDH. Điều quan trọng là so cùng convention. Một số model thư viện có tool transform hoặc base transform mặc định, nên trước khi kết luận code của mình sai, hãy in robot.base, robot.tool, và bảng link.

Dùng Pinocchio 3.x để load URDF

Pinocchio là thư viện C++/Python mạnh cho kinematics và dynamics. Theo tài liệu ROS Rolling tới mốc 06/2026, dòng Pinocchio 3.x mới nhất được liệt kê là 3.9.0. API Python cơ bản để load URDF vẫn rất gọn:

import pinocchio as pin

urdf_filename = "ur5_robot.urdf"
model = pin.buildModelFromUrdf(urdf_filename)
data = model.createData()

q = pin.neutral(model)
pin.forwardKinematics(model, data, q)

for name, placement in zip(model.names, data.oMi):
    print(name, placement.translation.T)

Pinocchio model khác DH table ở chỗ nó parse cả cây URDF: fixed joint, inertial frame, visual/collision geometry nếu bạn load thêm geometry model, và nhiều frame phụ như base, flange, tool0. Vì vậy khi so FK, nên lấy đúng frame:

frame_id = model.getFrameId("tool0")
pin.framesForwardKinematics(model, data, q)
T_base_tool0 = data.oMf[frame_id]
print(T_base_tool0.homogeneous)

Trong controller công nghiệp, Pinocchio phù hợp khi bạn cần Jacobian, dynamics, centroidal quantities, contact hoặc robot nhiều nhánh. Với bài này, ta chỉ dùng nó như ground truth URDF parser để kiểm tra rằng model từ scratch của mình không đi lệch convention.

Checklist tự kiểm

Trước khi sang bài IK, hãy tự trả lời:

• q của bạn là radian hay degree?
• FK của Python và C++ có giống nhau ở q = zeros(6) không?
• FK có giống nhau ở một cấu hình không đối xứng như [0.2, -1.1, 1.4, -0.7, 0.5, 0.3] không?
• Bạn đang so base, base_link, hay world?
• Bạn đang so flange, tool0, hay TCP custom?
• Bảng của bạn là standard DH hay modified DH?
• Có theta_offset hoặc calibration correction nào đang bị bỏ qua không?

Nếu checklist này ổn, bạn đã có nền đủ tốt để học IK. Trong bài về frames và jogging, các frame base, tool, world sẽ trở lại dưới góc nhìn điều khiển vận tốc. Bên ngoài series, bạn cũng có thể đọc thêm bài ROS2 Control Hardware Interface để thấy model robot đi vào tầng driver như thế nào.

Nguồn tham khảo

• Universal Robots DH parameters: bảng DH UR5 classic và UR5e.
• Universal Robots ROS 2 robot frames: base_link, base, flange, tool0.
• Modern Robotics về homogeneous transforms: SE(3) và ma trận 4x4.
• Eigen Geometry module: Transform, Isometry3d, matrix representation.
• Robotics Toolbox for Python documentation: DHRobot, URDF model, Swift visualizer.
• Pinocchio Rolling 3.9.0 changelog và loading model example: mốc version 3.x, buildModelFromUrdf, forwardKinematics, model/data workflow.

Bài viết liên quan

• Dựng project C++ cho robot controller: CMake & Eigen
• Forward Kinematics, Inverse Kinematics và Jacobian cho robot arm
• Frames và Jogging: base, tool, world trong robot arm controller