Vì sao bài này quan trọng?
Hãy tưởng tượng bạn đang dùng teach pendant điều khiển robot UR5 để tiếp cận một chi tiết hàn. Bạn nhấn "di chuyển +X" — và robot trượt sang ngang theo hướng hoàn toàn không mong đợi. Bạn điều chỉnh lại, nhấn "+Z", robot lại đi chéo. Bạn bắt đầu tự hỏi robot có bị lỗi không.
Vấn đề không phải robot. Vấn đề là bạn đang jog trong Base Frame nhưng nghĩ mình đang jog trong Tool Frame.
Đây là lỗi phổ biến nhất của kỹ sư mới làm việc với robot arm công nghiệp. Ba hệ tọa độ — Base Frame, Tool/TCP Frame, và User/Workobject Frame — là nền tảng để hiểu tại sao robot di chuyển theo cách đó, và làm chủ mọi lệnh điều khiển.
Trong bài này, chúng ta sẽ:
- Phân tích rõ 3 hệ tọa độ với sơ đồ ASCII minh họa
- Calibrate TCP tùy chỉnh cho bất kỳ end-effector nào
- Jog robot bằng code theo từng frame với
jogStart() - Dạy waypoint bằng tay với
freedriveMode(free_axes=[1,1,1,0,0,0]) - Định nghĩa Workobject Frame để pallet program tái sử dụng qua mọi layout nhà máy
Roadmap series
| Phần | Bài | Nội dung chính |
|---|---|---|
| 1 | FK, IK và Jacobian | DH parameters, getForwardKinematics, getInverseKinematics, singularity |
| 2 (bài này) | Frames, Jogging & TCP | Base/Tool/User frames, jogStart, setTcp, freedriveMode |
| 3 | Motion Primitives: MoveJ, MoveL, MoveC, MoveP | Từng lệnh motion, khi nào dùng gì |
| 4 | Velocity Blending & Trajectory | Trapezoid/S-curve profile, blending radius |
| 5 | ROS2 & RTDE Integration | Full stack: ROS2 node, RTDE loop, ur_ros_rtde |
1. Ba hệ tọa độ cơ bản
Trước khi chạm đến bất kỳ dòng code nào, cần hiểu rõ 3 "ngôn ngữ tọa độ" mà robot công nghiệp sử dụng.
BASE FRAME (cố định) TOOL FRAME (gắn TCP)
────────────────────── ──────────────────────
↑ Z ↑ Z_tool
│ │ (hướng ra trước theo công cụ)
──────┼──────→ Y ──────┼──────→ X_tool
/│ /│
/ │ / │
↙ │ ↙ │
X └── Gắn ở sàn Y_tool └── Gắn ở TCP
1.1. Base Frame — Nền tảng cố định
Base Frame là hệ tọa độ gốc, gắn cố định vào điểm gắn robot (thường là sàn nhà máy hoặc bệ đỡ). Mọi tọa độ Cartesian bạn đưa vào moveL() hay moveJ_IK() đều tính trong Base Frame mặc định.
Đặc điểm:
- Không thay đổi khi robot di chuyển — là reference tuyệt đối
- Gốc
(0, 0, 0)nằm ở tâm mặt phẳng gắn robot (mounting surface) - Z-axis hướng lên theo trục robot
- Tất cả pose trong ur_rtde mặc định là Base Frame
# Tất cả lệnh này đều ngầm dùng Base Frame
rtde_c.moveL([0.3, -0.3, 0.4, 0, 3.14, 0]) # pose tuyệt đối trong Base Frame
rtde_c.getForwardKinematics(q) # output là Base Frame pose
rtde_c.getInverseKinematics(pose, qnear=q) # input là Base Frame pose
Khi nào dùng Base Frame:
- Program đường đi tuyệt đối khi robot và workpiece đều cố định
- Safety boundaries — luôn định nghĩa keepout zone trong Base Frame để tránh nhầm lẫn
- Di chuyển về "home position" — dùng absolute Base Frame pose
1.2. Tool Frame / TCP — Hệ tọa độ đầu công cụ
TCP (Tool Center Point) là điểm tác động của end-effector — đầu hàn, ngón gripper, mũi hút chân không... Tool Frame là hệ tọa độ gắn với TCP đó.
Flange (output shaft của robot)
│
│ tcp_offset = [dx, dy, dz, rx, ry, rz]
│ (6-DOF pose: offset từ flange center đến TCP)
│
└──────────────→ TCP
↑ Z_tool (hướng ra trước theo công cụ)
│
X_tool ──→ (hướng mở ngón gripper)
Y_tool
Khi jog trong Tool Frame:
- +Z_tool → TCP tiến ra trước theo hướng công cụ, không phải hướng Base
- +X_tool → di chuyển dọc theo chiều ngang của gripper
- Hữu ích: tiếp cận chi tiết luôn đi thẳng vào từ phía công cụ, bất kể orientation hiện tại
# Sau khi setTcp(), mọi lệnh Cartesian tính từ TCP mới
tcp_gripper = [0, 0, 0.150, 0, 0, 0] # gripper dài 150mm dọc trục Z
rtde_c.setTcp(tcp_gripper)
# Giờ moveL sẽ điều khiển đến TCP (đầu gripper), không phải flange
rtde_c.moveL([0.3, -0.2, 0.35, 0, 3.14, 0])
Khi nào dùng Tool Frame:
- Tiếp cận chi tiết theo hướng vuông góc (hàn, gắp, vặn vít)
- Jog để dạy waypoint khi nhìn vào công cụ, không nhìn vào base
- Spiral path quanh trục Z công cụ (khoan, mài tròn)
1.3. User Frame / Workobject — Hệ tọa độ tương đối jig
User Frame (còn gọi là Workobject) là hệ tọa độ do người dùng tự định nghĩa, thường gắn với fixture/jig chứa workpiece.
↑ Z_user
[JIG/FIXTURE] │
┌─────────────┐ ├──→ X_user (dọc cạnh dài jig)
│ Part A B │ /
│ Part C D │ / Y_user (cạnh ngắn jig)
└─────────────┘
↑ Origin được dạy
tại góc jig
Sức mạnh thực sự của User Frame:
Giả sử bạn lập trình gắp 100 sản phẩm từ pallet. Bạn đã dạy 100 waypoint. Một ngày jig bị va chạm và lệch 50 mm. Kết quả:
- Dùng Base Frame: phải dạy lại 100 điểm — mất vài giờ
- Dùng User Frame: chỉ cần re-teach origin của User Frame — 100 waypoint tự động cập nhật theo
Đây chính xác là lý do tại sao mọi nhà máy sản xuất nghiêm túc đều dùng User Frame cho mọi cell layout.
2. setTcp() — Cài đặt TCP tùy chỉnh
Trước khi jog theo Tool Frame, robot cần biết TCP của end-effector nằm ở đâu so với output flange.
2.1. API cơ bản
import rtde_control
import numpy as np
import math
rtde_c = rtde_control.RTDEControlInterface("192.168.1.100")
# tcp_offset = [x, y, z, rx, ry, rz]
# — Transformation từ flange center đến TCP
# — Đơn vị: mét và radian (rotation vector)
# Gripper thẳng, dài 150mm dọc trục Z flange
tcp_straight_gripper = [0, 0, 0.150, 0, 0, 0]
rtde_c.setTcp(tcp_straight_gripper)
# Welding torch lệch 120mm theo Z, xoay 90° quanh Y
tcp_welding_torch = [0, 0, 0.120, 0, math.pi/2, 0]
rtde_c.setTcp(tcp_welding_torch)
# Camera gắn offset: 30mm theo X, 80mm theo Z
tcp_camera = [0.030, 0, 0.080, 0, 0, 0]
rtde_c.setTcp(tcp_camera)
# Reset về flange center (mặc định)
rtde_c.setTcp([0, 0, 0, 0, 0, 0])
Lưu ý quan trọng:
setTcp()trong ur_rtde ảnh hưởng đến mọi lệnh điều khiển Cartesian của session hiện tại. Nó không lưu vào robot controller vĩnh viễn — cần gọi lại mỗi khi khởi tạoRTDEControlInterface.
2.2. Calibrate TCP bằng phương pháp 4 điểm
Khi không biết chính xác kích thước end-effector, dùng phương pháp 4 điểm để đo TCP tự động:
Nguyên lý: Đưa TCP chạm vào cùng một điểm cố định trong không gian từ ít nhất 4 hướng khác nhau. Robot ghi nhận 4 pose của flange. Vì TCP là điểm chung → giải hệ phương trình tìm offset.
import rtde_control
import rtde_receive
import numpy as np
from scipy.optimize import minimize
from scipy.spatial.transform import Rotation
rtde_c = rtde_control.RTDEControlInterface("192.168.1.100")
rtde_r = rtde_receive.RTDEReceiveInterface("192.168.1.100")
def collect_tcp_calibration_poses(n_poses: int = 4) -> list:
"""
Thu thập n_poses flange poses khi TCP chạm cùng 1 điểm cố định.
Operator đưa TCP chạm vào điểm chuẩn (mũi bút nhọn) từ n hướng khác nhau.
"""
# Reset TCP về flange trước khi đo
rtde_c.setTcp([0, 0, 0, 0, 0, 0])
poses = []
print(f"Chuẩn bị 1 điểm chuẩn cố định (ví dụ: đầu mũi nhọn trên bàn).")
for i in range(n_poses):
input(f"\nPose {i+1}/{n_poses}: Đưa TCP chạm điểm chuẩn "
f"(orientation KHÁC pose trước ≥45°), nhấn Enter...")
# Lấy flange pose hiện tại trong Base Frame
flange_pose = rtde_r.getActualTCPPose() # khi TCP=[0,0,0,0,0,0] → là flange pose
poses.append(flange_pose)
print(f" Ghi: {[f'{v*1000:.1f}' for v in flange_pose[:3]]} mm")
return poses
def compute_tcp_from_4_poses(flange_poses: list) -> np.ndarray:
"""
Tính TCP offset từ danh sách flange poses bằng least-squares optimization.
Tất cả flange_poses phải có TCP chạm cùng 1 điểm.
"""
def cost_fn(tcp_local):
"""
Nếu tcp_local đúng, tất cả flange_poses tính ra cùng 1 điểm TCP trong Base Frame.
Cost = variance của các điểm TCP tính được.
"""
tcp_world_points = []
for fp in flange_poses:
pos = np.array(fp[:3])
rot = Rotation.from_rotvec(fp[3:])
# TCP trong Base Frame = flange_pos + R_flange * tcp_local
tcp_world = pos + rot.apply(np.array(tcp_local))
tcp_world_points.append(tcp_world)
pts = np.array(tcp_world_points)
return np.sum(np.var(pts, axis=0))
result = minimize(cost_fn, x0=[0, 0, 0.1], method='Nelder-Mead',
options={'xatol': 1e-6, 'fatol': 1e-8, 'maxiter': 10000})
residual_mm = np.sqrt(result.fun) * 1000
print(f"TCP calibrated: {[f'{v*1000:.2f}' for v in result.x]} mm")
print(f"Residual error: {residual_mm:.3f} mm")
return result.x
# Workflow:
# poses = collect_tcp_calibration_poses(4)
# tcp_offset = compute_tcp_from_4_poses(poses)
# rtde_c.setTcp(list(tcp_offset) + [0, 0, 0]) # orientation từ PolyScope hoặc đo thủ công
Thực tế: PolyScope (UI trên robot UR) có chức năng Auto-calibrate TCP trong
Installation > TCP. Bạn chỉ cần chỉ robot chạm cùng một điểm 4 lần qua giao diện — robot tự tính. Kết quả từ PolyScope chính xác hơn vì dùng kinematic model đã calibrate từ nhà máy.
3. jogStart() — Jogging bằng code theo từng frame
jogStart() là API mô phỏng teach pendant trong Python — cho phép jog robot theo đúng frame mong muốn với tốc độ tùy chỉnh.
3.1. Cú pháp đầy đủ
rtde_c.jogStart(
speeds, # [vx, vy, vz, wrx, wry, wrz]
feature, # FEATURE_BASE | FEATURE_TOOL | FEATURE_CUSTOM
acceleration=0.5, # gia tốc [m/s²]
custom_feature=None # 6-DOF pose của custom frame (chỉ cho FEATURE_CUSTOM)
)
rtde_c.jogStop(acceleration=0.5) # Dừng lại
speeds là vector 6 chiều:
[vx, vy, vz]— vận tốc dịch chuyển TCP trong frame được chọn (m/s)[wrx, wry, wrz]— vận tốc xoay quanh các trục của frame (rad/s)
3.2. Jog trong Base Frame
import rtde_control
import time
rtde_c = rtde_control.RTDEControlInterface("192.168.1.100")
FEAT_BASE = rtde_control.RTDEControlInterface.FEATURE_BASE
# Jog +X trong Base Frame ở 50mm/s trong 2 giây
rtde_c.jogStart([0.05, 0, 0, 0, 0, 0], FEAT_BASE)
time.sleep(2.0)
rtde_c.jogStop()
# Jog -Z trong Base Frame ở 30mm/s trong 1.5 giây
rtde_c.jogStart([0, 0, -0.03, 0, 0, 0], FEAT_BASE)
time.sleep(1.5)
rtde_c.jogStop()
# Jog xoay quanh Z_base ở 0.2 rad/s trong 1 giây
rtde_c.jogStart([0, 0, 0, 0, 0, 0.2], FEAT_BASE)
time.sleep(1.0)
rtde_c.jogStop()
3.3. Jog trong Tool Frame — Quan trọng nhất
Đây là feature bạn sẽ dùng nhiều nhất khi lập trình tiếp cận chi tiết:
FEAT_TOOL = rtde_control.RTDEControlInterface.FEATURE_TOOL
# BẮT BUỘC: setTcp() đúng trước khi jog theo Tool Frame
tcp_gripper = [0, 0, 0.150, 0, 0, 0] # gripper 150mm dọc Z
rtde_c.setTcp(tcp_gripper)
# Jog tiến ra trước theo hướng công cụ (+Z_tool)
# Dù robot đang ở bất kỳ orientation nào, TCP luôn đi "ra trước theo đầu gripper"
rtde_c.jogStart([0, 0, 0.05, 0, 0, 0], FEAT_TOOL) # 50mm/s +Z_tool
time.sleep(1.0)
rtde_c.jogStop()
# Jog xoay quanh trục Z công cụ (điều chỉnh góc gripper)
rtde_c.jogStart([0, 0, 0, 0, 0, 0.3], FEAT_TOOL) # 0.3 rad/s quanh Z_tool
time.sleep(0.5)
rtde_c.jogStop()
# Jog ngang theo X_tool (điều chỉnh vị trí ngang của gripper)
rtde_c.jogStart([0.02, 0, 0, 0, 0, 0], FEAT_TOOL) # 20mm/s +X_tool
time.sleep(1.5)
rtde_c.jogStop()
Lý do setTcp() phải đúng trước: nếu TCP offset sai, robot jog theo tool frame ảo — TCP thật di chuyển theo quỹ đạo tròn thay vì đường thẳng, có thể gây va chạm nghiêm trọng.
3.4. Jog trong User Frame / Custom Feature
import math
FEAT_CUSTOM = rtde_control.RTDEControlInterface.FEATURE_CUSTOM
# Định nghĩa User Frame: pose của origin frame trong Base Frame
# Jig đặt ở x=0.4, y=0.2, z=0, xoay 30° quanh Z_base
user_frame = [0.4, 0.2, 0.0, 0, 0, math.pi/6]
# Jog +X theo User Frame — TCP di chuyển dọc theo cạnh dài của jig
rtde_c.jogStart(
[0.05, 0, 0, 0, 0, 0],
FEAT_CUSTOM,
custom_feature=user_frame # BẮT BUỘC truyền custom_feature cho FEATURE_CUSTOM
)
time.sleep(2.0)
rtde_c.jogStop()
# Jog +Y theo User Frame — TCP di chuyển ngang qua jig
rtde_c.jogStart(
[0, 0.03, 0, 0, 0, 0],
FEAT_CUSTOM,
custom_feature=user_frame
)
time.sleep(2.0)
rtde_c.jogStop()
3.5. Interactive keyboard jogging — Giống teach pendant thật
import rtde_control
import sys
import tty
import termios
import time
rtde_c = rtde_control.RTDEControlInterface("192.168.1.100")
rtde_c.setTcp([0, 0, 0.150, 0, 0, 0])
FEAT_TOOL = rtde_control.RTDEControlInterface.FEATURE_TOOL
JOG_SPEED_TRANS = 0.03 # 30mm/s dịch chuyển
JOG_SPEED_ROT = 0.20 # 0.2 rad/s xoay
KEY_MAP = {
'w': ([0, 0, JOG_SPEED_TRANS, 0, 0, 0], "+Z tool (tiếp cận)"),
's': ([0, 0, -JOG_SPEED_TRANS, 0, 0, 0], "-Z tool (rút lui)"),
'a': ([-JOG_SPEED_TRANS, 0, 0, 0, 0, 0], "-X tool"),
'd': ([ JOG_SPEED_TRANS, 0, 0, 0, 0, 0], "+X tool"),
'q': ([0, JOG_SPEED_TRANS, 0, 0, 0, 0], "+Y tool"),
'e': ([0, -JOG_SPEED_TRANS, 0, 0, 0, 0], "-Y tool"),
'z': ([0, 0, 0, JOG_SPEED_ROT, 0, 0], "+Rx (lăn)"),
'x': ([0, 0, 0, -JOG_SPEED_ROT, 0, 0], "-Rx"),
'c': ([0, 0, 0, 0, JOG_SPEED_ROT, 0], "+Ry (nghiêng)"),
'v': ([0, 0, 0, 0, -JOG_SPEED_ROT, 0], "-Ry"),
}
def get_key():
fd = sys.stdin.fileno()
old = termios.tcgetattr(fd)
try:
tty.setraw(fd)
return sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old)
print("Keyboard jog (Tool Frame) — W/S: ±Z, A/D: ±X, Q/E: ±Y")
print("Z/X: ±Rx, C/V: ±Ry | Ctrl+C để thoát")
is_jogging = False
try:
while True:
key = get_key()
if key == '\x03':
break
if key in KEY_MAP:
speeds, label = KEY_MAP[key]
print(f" → {label}")
rtde_c.jogStart(speeds, FEAT_TOOL)
is_jogging = True
else:
if is_jogging:
rtde_c.jogStop()
is_jogging = False
finally:
if is_jogging:
rtde_c.jogStop()
rtde_c.disconnect()
print("Đã dừng và ngắt kết nối.")
4. freedriveMode() — Dạy TCP bằng tay
freedriveMode() cho phép operator đẩy robot bằng tay để dạy waypoint thay vì jog từng trục. Đây là kỹ thuật phổ biến trong contact task teaching (pick-and-place, assembly, surface following).
4.1. Tham số free_axes
rtde_c.freedriveMode(
freeAxes=[tx, ty, tz, rx, ry, rz], # 1 = tự do di chuyển, 0 = khóa
feature="base", # "base", "tool", hoặc pose tùy chỉnh
)
# Dừng freedrive:
rtde_c.endFreedriveMode()
┌─────────────────────────────────────────────────────────────────────┐
│ freeAxes │ Ý nghĩa │ Ứng dụng │
├─────────────────────────────────────────────────────────────────────┤
│ [1,1,1,1,1,1] │ Tất cả 6 DOF tự do │ Dạy tổng quát │
│ [1,1,1,0,0,0] │ XYZ tự do, xoay khóa │ Dạy position, │
│ │ │ giữ orientation │
│ [0,0,1,0,0,0] │ Chỉ Z tự do │ Điều chỉnh chiều│
│ │ │ sâu tiếp xúc │
│ [1,1,0,0,0,0] │ Chỉ XY tự do │ Dạy trên mặt │
│ │ │ phẳng │
│ [0,0,0,0,0,1] │ Chỉ xoay Z │ Điều chỉnh góc │
│ │ │ công cụ │
└─────────────────────────────────────────────────────────────────────┘
4.2. Constrained freedrive — Khóa orientation khi dạy TCP
Use case phổ biến nhất: dạy vị trí của gripper mà không để orientation bị thay đổi.
import rtde_control
import rtde_receive
rtde_c = rtde_control.RTDEControlInterface("192.168.1.100")
rtde_r = rtde_receive.RTDEReceiveInterface("192.168.1.100")
def teach_waypoint_constrained(label: str,
free_axes=[1, 1, 1, 0, 0, 0]) -> list:
"""
Dạy 1 waypoint bằng freedrive với ràng buộc trục.
Mặc định: chỉ XYZ tự do, 3 trục xoay bị khóa.
Operator đẩy robot đến vị trí → nhấn Enter để ghi lại.
"""
print(f"\n--- Dạy waypoint: {label} ---")
print(f"free_axes = {free_axes} (1=tự do, 0=khóa)")
print("→ Đẩy robot đến vị trí mong muốn")
print("→ Nhấn Enter khi xong...")
# Kích hoạt freedrive với constraint trong Tool Frame
rtde_c.freedriveMode(
freeAxes=free_axes,
feature="tool" # di chuyển tự do trong Tool Frame
)
input() # Chờ operator xác nhận
# Dừng freedrive ngay — QUAN TRỌNG: không được bỏ qua
rtde_c.endFreedriveMode()
pose = rtde_r.getActualTCPPose()
q = rtde_r.getActualQ()
print(f" ✓ Waypoint: [{', '.join(f'{v*1000:.1f}' for v in pose[:3])}] mm")
return pose
# Dạy 3 waypoints với orientation cố định
waypoints = []
for label in ["Pick-A", "Pick-B", "Place-C"]:
wp = teach_waypoint_constrained(label)
waypoints.append(wp)
# Lưu ra file để dùng lại
import json
with open("waypoints.json", "w") as f:
json.dump(waypoints, f, indent=2)
print(f"\nĐã lưu {len(waypoints)} waypoints vào waypoints.json")
4.3. Freedrive để record đường cong phức tạp
Cho curved path teaching (hàn đường cong phức tạp, spray coating bề mặt 3D):
import time
def record_continuous_path(duration_s=8.0, sample_hz=50) -> list:
"""
Ghi liên tục TCP pose trong khi operator di chuyển robot tay.
Dùng cho path teaching khi đường đi phức tạp khó mô tả bằng waypoints.
"""
dt = 1.0 / sample_hz
path = []
print(f"Bắt đầu ghi path trong {duration_s}s — hãy di chuyển robot...")
rtde_c.freedriveMode(freeAxes=[1, 1, 1, 1, 1, 1]) # tất cả tự do
start = time.time()
while time.time() - start < duration_s:
pose = rtde_r.getActualTCPPose()
path.append(pose[:]) # copy toàn bộ 6-DOF
time.sleep(dt)
rtde_c.endFreedriveMode()
print(f"Đã ghi {len(path)} điểm ({len(path) / sample_hz:.1f}s @ {sample_hz}Hz)")
return path
# Dùng path đã ghi để replay:
# for pose in recorded_path:
# rtde_c.moveL(pose, speed=0.05, acceleration=0.3)
5. Workobject Frame cho Pallet Program
Đây là ứng dụng thực tế nhất của User Frame: lập trình gắp nhiều sản phẩm từ pallet mà không cần dạy lại khi jig di chuyển hoặc layout nhà máy thay đổi.
5.1. Định nghĩa Workobject bằng 3 điểm
Phương pháp 3 điểm (tiêu chuẩn công nghiệp):
Point 1 — Origin: xác định (0,0,0) của User Frame
Point 2 — X-axis: điểm dọc theo X+ của jig (cạnh dài)
Point 3 — XY-plane: điểm trên mặt XY, phía Y+ (cạnh ngắn)
P2 (X-axis)
↗
P1 (origin)─────────────────────
│ │
│ ← Bề mặt jig (XY-plane) │
│ │
└───────────────────────────────┘
P3 (XY-plane, Y+ side)
import numpy as np
from scipy.spatial.transform import Rotation
def define_workobject_3points() -> list:
"""
Dạy User Frame bằng 3 điểm:
P1 = Origin của frame
P2 = Điểm trên trục X+ (dọc cạnh dài jig)
P3 = Điểm trên mặt XY, phía Y+
Trả về 6-DOF pose của User Frame trong Base Frame.
"""
points = []
labels = [
"P1 — Origin (góc jig)",
"P2 — X-axis (dọc cạnh dài jig từ P1)",
"P3 — XY-plane (cạnh ngắn jig, phía Y+)",
]
for label in labels:
input(f"\nDi chuyển TCP đến {label}, nhấn Enter...")
pose = rtde_r.getActualTCPPose()
points.append(np.array(pose[:3]))
print(f" → {[f'{v*1000:.1f}' for v in pose[:3]]} mm")
origin, px, py = points
# Tính các trục của User Frame từ 3 điểm
x_axis = px - origin
x_axis /= np.linalg.norm(x_axis)
xy_vec = py - origin
z_axis = np.cross(x_axis, xy_vec)
z_axis /= np.linalg.norm(z_axis)
y_axis = np.cross(z_axis, x_axis)
# Rotation matrix → rotation vector (axis-angle)
R = np.column_stack([x_axis, y_axis, z_axis])
rvec = Rotation.from_matrix(R).as_rotvec()
frame_pose = list(origin) + list(rvec)
print(f"\nUser Frame: {[f'{v:.4f}' for v in frame_pose]}")
return frame_pose
5.2. Pallet program fixture-relative và tái sử dụng
from scipy.spatial.transform import Rotation
def transform_to_base(local_pose: list, frame_pose: list) -> list:
"""
Chuyển pose từ User Frame sang Base Frame.
Cần thiết vì ur_rtde moveL() luôn dùng Base Frame.
"""
frame_pos = np.array(frame_pose[:3])
frame_rot = Rotation.from_rotvec(frame_pose[3:])
local_pos = np.array(local_pose[:3])
local_rot = Rotation.from_rotvec(local_pose[3:])
base_pos = frame_pos + frame_rot.apply(local_pos)
base_rot = frame_rot * local_rot
return list(base_pos) + list(base_rot.as_rotvec())
def run_pallet_pick(user_frame: list,
rows=4, cols=5,
spacing_x=0.080, # 80mm giữa các cột
spacing_y=0.060, # 60mm giữa các hàng
pick_z=0.010, # chiều cao bề mặt sản phẩm trong User Frame
approach_dz=0.060): # tiếp cận từ 60mm phía trên
"""
Gắp 4×5 = 20 sản phẩm từ pallet.
Tất cả tọa độ trong User Frame → chỉ cần cập nhật user_frame khi jig di chuyển.
"""
rtde_c.setTcp([0, 0, 0.120, 0, 0, 0]) # gripper 120mm
for row in range(rows):
for col in range(cols):
# Tọa độ trong User Frame (tính từ góc jig)
x_local = col * spacing_x + 0.020
y_local = row * spacing_y + 0.020
approach_local = [x_local, y_local, pick_z + approach_dz, 0, 0, 0]
pick_local = [x_local, y_local, pick_z, 0, 0, 0]
# Chuyển sang Base Frame để gọi moveL
approach_base = transform_to_base(approach_local, user_frame)
pick_base = transform_to_base(pick_local, user_frame)
rtde_c.moveL(approach_base, speed=0.30, acceleration=0.8)
rtde_c.moveL(pick_base, speed=0.05, acceleration=0.3)
# ... đóng gripper ...
rtde_c.moveL(approach_base, speed=0.10, acceleration=0.5)
print(f"Gắp: row={row}, col={col} "
f"(x={x_local*1000:.0f}mm, y={y_local*1000:.0f}mm)")
# Dùng: nếu jig di chuyển → chỉ cần re-teach user_frame
# user_frame = define_workobject_3points()
# run_pallet_pick(user_frame)
Đây là sức mạnh của fixture-relative programming: code không thay đổi — chỉ user_frame thay đổi khi layout thay đổi.
6. Những điều cần nhớ
Base Frame là default, không thay đổi. Mọi pose trong ur_rtde mặc định là Base Frame trừ khi bạn chỉ định feature khác.
setTcp() BẮT BUỘC trước jogStart(FEATURE_TOOL). Không set TCP đúng → jog theo tool frame tính từ flange → TCP thật di chuyển theo vòng tròn, nguy cơ va chạm cao.
FEATURE_CUSTOM cần custom_feature pose. Nếu không truyền custom_feature khi dùng FEATURE_CUSTOM, hành vi là undefined. Luôn truyền đầy đủ.
endFreedriveMode() không được bỏ qua. Robot ở chế độ freedrive mãi nếu không gọi hàm này. Đặt trong finally block để đảm bảo luôn được gọi kể cả khi exception xảy ra.
User Frame lưu ra file config. Luôn lưu User Frame của từng jig vào file JSON/YAML. Khi jig bị di chuyển → re-teach và cập nhật file — đừng hardcode trong code.
Jog speed: bắt đầu thấp. Luôn test với 10-20mm/s trước. ur_rtde clamp tốc độ về giới hạn an toàn nhưng không ngăn va chạm nếu robot đang ở gần vật cản.
Kết luận
Bài này đã đi qua đầy đủ 3 coordinate frames trong robot arm công nghiệp: Base Frame cố định là reference tuyệt đối, Tool/TCP Frame được định nghĩa qua setTcp() với offset 6-DOF từ flange đến đầu công cụ, và User/Workobject Frame được đăng ký như FEATURE_CUSTOM trong jogStart().
Điểm khác biệt thực sự giữa kỹ sư giỏi và kỹ sư trung bình không phải là biết nhiều lệnh API hơn — mà là luôn tự hỏi "tôi đang di chuyển trong frame nào?" trước khi viết bất kỳ lệnh motion nào.
Bài tiếp theo sẽ đi vào Motion Primitives: khi nào dùng moveJ, khi nào dùng moveL, moveC và moveP — và tại sao chọn sai lệnh motion có thể gây singularity hoặc va chạm không mong đợi.
