MoveJ và MoveL: Joint Motion và Linear Motion chi tiết

Trong bài trước, chúng ta đã so sánh Joint Space và Cartesian Space planning ở mức lý thuyết. Bây giờ đến lúc thực hành — MoveJ và MoveL là hai lệnh cơ bản nhất mà bất kỳ kỹ sư robot nào cũng phải thành thạo. Chúng là hiện thực hóa trực tiếp của Joint Space và Cartesian Space trajectory.

Trong bài này, chúng ta sẽ đi sâu vào cơ chế hoạt động, tham số, giới hạn, và demo với URScript cùng Python.

MoveJ — Joint Interpolation Motion

Cơ chế hoạt động

MoveJ nội suy trong không gian khớp (Joint Space). Controller tính quỹ đạo riêng cho từng khớp:

1. Tính hiệu góc: $\Delta q_i = q_{target,i} - q_{current,i}$
2. Xác định khớp cần thời gian dài nhất (khớp "bottleneck")
3. Đồng bộ tất cả khớp để kết thúc cùng lúc
4. Áp dụng velocity profile (thường là trapezoidal hoặc S-curve)

Kết quả: Mỗi khớp di chuyển mượt mà, nhưng đường TCP trong Cartesian cong và không dự đoán được.

Khi nào dùng MoveJ?

URScript MoveJ

# URScript — chạy trên UR controller
# movej(q, a=1.4, v=1.05, t=0, r=0)
# q: joint angles [rad]
# a: acceleration [rad/s²]
# v: velocity [rad/s]
# t: time [s] (0 = auto-compute)
# r: blend radius [m] (0 = stop at point)

# Di chuyển đến home position
movej([0, -1.5708, 1.5708, -1.5708, -1.5708, 0], a=1.4, v=1.05)

# Di chuyển chậm, an toàn
movej(get_inverse_kin(p[0.4, 0.3, 0.2, 0, 3.14, 0]),
      a=0.5, v=0.3)

# Di chuyển nhanh với blend radius (không dừng tại điểm)
movej(q_waypoint1, a=1.4, v=1.05, r=0.05)
movej(q_waypoint2, a=1.4, v=1.05, r=0.05)
movej(q_final, a=1.4, v=1.05, r=0)  # Dừng tại điểm cuối

Python với roboticstoolbox

import roboticstoolbox as rtb
import numpy as np
import matplotlib.pyplot as plt

ur5e = rtb.models.UR5()

# Start và target configurations
q_start = np.array([0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0])
q_target = np.array([np.pi/2, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, np.pi/4])

# MoveJ: Joint space trajectory (quintic polynomial)
traj = rtb.jtraj(q_start, q_target, t=100)

# Tính TCP positions dọc quỹ đạo
tcp_positions = []
for q in traj.q:
    T = ur5e.fkine(q)
    tcp_positions.append(T.t)
tcp_positions = np.array(tcp_positions)

# Visualize TCP path
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
ax.plot(tcp_positions[:, 0], tcp_positions[:, 1], tcp_positions[:, 2],
        'b-', linewidth=2, label='MoveJ TCP path')
ax.scatter(*tcp_positions[0], c='green', s=100, zorder=5, label='Start')
ax.scatter(*tcp_positions[-1], c='red', s=100, zorder=5, label='Target')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('MoveJ — TCP Path (curved, unpredictable)')
ax.legend()
plt.tight_layout()
plt.savefig('movej_tcp_path.png', dpi=150)
plt.show()

# Plot joint trajectories
fig, axes = plt.subplots(3, 1, figsize=(12, 8), sharex=True)
for i in range(6):
    axes[0].plot(np.degrees(traj.q[:, i]), label=f'Joint {i+1}')
    axes[1].plot(np.degrees(traj.qd[:, i]), label=f'Joint {i+1}')
    axes[2].plot(np.degrees(traj.qdd[:, i]), label=f'Joint {i+1}')

axes[0].set_ylabel('Position (deg)')
axes[0].set_title('MoveJ Joint Profiles')
axes[0].legend(ncol=3)
axes[0].grid(True)
axes[1].set_ylabel('Velocity (deg/s)')
axes[1].grid(True)
axes[2].set_ylabel('Acceleration (deg/s²)')
axes[2].set_xlabel('Time step')
axes[2].grid(True)
plt.tight_layout()
plt.savefig('movej_joint_profiles.png', dpi=150)
plt.show()

MoveL — Linear Interpolation Motion

Cơ chế hoạt động

MoveL nội suy trong không gian Cartesian. Controller:

1. Tính đường thẳng trong Cartesian giữa TCP hiện tại và target
2. Chia thành nhiều điểm nhỏ (interpolation points)
3. Tại mỗi điểm: giải IK để tìm góc khớp
4. Áp dụng velocity profile trên khoảng cách Cartesian

Kết quả: TCP đi theo đường thẳng chính xác trong 3D, nhưng các khớp có thể di chuyển phức tạp.

Khi nào dùng MoveL?

URScript MoveL

# URScript
# movel(pose, a=1.2, v=0.25, t=0, r=0)
# pose: target TCP pose [x, y, z, rx, ry, rz] (axis-angle)
# a: acceleration [m/s²]
# v: velocity [m/s]
# t: time [s] (0 = auto)
# r: blend radius [m]

# Di chuyển thẳng đến vị trí gắp
movel(p[0.4, 0.3, 0.1, 0, 3.14, 0], a=1.2, v=0.1)

# Tiếp cận chậm (insertion)
movel(p[0.4, 0.3, 0.05, 0, 3.14, 0], a=0.5, v=0.02)

# Rút lên (retreat) — nhanh hơn
movel(p[0.4, 0.3, 0.2, 0, 3.14, 0], a=1.2, v=0.25)

Python implementation

import roboticstoolbox as rtb
import numpy as np
import matplotlib.pyplot as plt
from spatialmath import SE3

ur5e = rtb.models.UR5()

# Start configuration
q_start = np.array([0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0])
T_start = ur5e.fkine(q_start)

# Target pose (Cartesian)
T_target = SE3(0.3, 0.4, 0.2) * SE3.Rx(np.pi)

# MoveL: Cartesian straight line
n_points = 100
tcp_positions = []
joint_traj = []

for i in range(n_points):
    s = i / (n_points - 1)

    # Linear position interpolation
    pos = T_start.t + s * (T_target.t - T_start.t)

    # SLERP orientation (simplified — using SE3.interp)
    T_interp = T_start.interp(T_target, s)

    # Solve IK at each point
    sol = ur5e.ikine_LM(T_interp, q0=q_start if i == 0 else joint_traj[-1])
    if sol.success:
        joint_traj.append(sol.q)
        T_actual = ur5e.fkine(sol.q)
        tcp_positions.append(T_actual.t)
    else:
        print(f"IK failed at s={s:.2f}")
        break

tcp_positions = np.array(tcp_positions)

# Verify straight line
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
ax.plot(tcp_positions[:, 0], tcp_positions[:, 1], tcp_positions[:, 2],
        'r-', linewidth=2, label='MoveL TCP path')
ax.scatter(*tcp_positions[0], c='green', s=100, zorder=5, label='Start')
ax.scatter(*tcp_positions[-1], c='red', s=100, zorder=5, label='Target')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('MoveL — TCP Path (straight line guaranteed)')
ax.legend()
plt.tight_layout()
plt.savefig('movel_tcp_path.png', dpi=150)
plt.show()

MoveJ vs MoveL — So sánh trực quan

import roboticstoolbox as rtb
import numpy as np
import matplotlib.pyplot as plt
from spatialmath import SE3

ur5e = rtb.models.UR5()

q_start = np.array([0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0])
q_target = np.array([np.pi/2, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, np.pi/4])

n_points = 100

# === MoveJ ===
movej_tcp = []
for i in range(n_points):
    s = i / (n_points - 1)
    q = q_start + s * (q_target - q_start)
    T = ur5e.fkine(q)
    movej_tcp.append(T.t)
movej_tcp = np.array(movej_tcp)

# === MoveL ===
T_start = ur5e.fkine(q_start)
T_end = ur5e.fkine(q_target)

movel_tcp = []
for i in range(n_points):
    s = i / (n_points - 1)
    pos = T_start.t + s * (T_end.t - T_start.t)
    movel_tcp.append(pos)
movel_tcp = np.array(movel_tcp)

# Side-by-side comparison
fig = plt.figure(figsize=(14, 6))
ax1 = fig.add_subplot(121, projection='3d')
ax1.plot(movej_tcp[:, 0], movej_tcp[:, 1], movej_tcp[:, 2],
         'b-', linewidth=2)
ax1.scatter(*movej_tcp[0], c='green', s=80)
ax1.scatter(*movej_tcp[-1], c='red', s=80)
ax1.set_title('MoveJ — Curved TCP Path')
ax1.set_xlabel('X'); ax1.set_ylabel('Y'); ax1.set_zlabel('Z')

ax2 = fig.add_subplot(122, projection='3d')
ax2.plot(movel_tcp[:, 0], movel_tcp[:, 1], movel_tcp[:, 2],
         'r-', linewidth=2)
ax2.scatter(*movel_tcp[0], c='green', s=80)
ax2.scatter(*movel_tcp[-1], c='red', s=80)
ax2.set_title('MoveL — Straight TCP Path')
ax2.set_xlabel('X'); ax2.set_ylabel('Y'); ax2.set_zlabel('Z')

plt.tight_layout()
plt.savefig('movej_vs_movel.png', dpi=150)
plt.show()

Tham số quan trọng

Speed và Acceleration

Blend Radius

Blend radius (r) cho phép robot không dừng lại tại waypoint mà "cắt góc" — tiết kiệm thời gian cycle.

# URScript — palletizing với blend radius
def palletize():
    # Approach (MoveJ, nhanh)
    movej(q_above_pallet, a=1.4, v=1.05, r=0.05)

    # Linear approach (MoveL, chậm, thẳng đứng)
    movel(p_place, a=1.2, v=0.1, r=0)

    # Đặt vật
    set_digital_out(0, True)  # Open gripper
    sleep(0.5)

    # Linear retreat (MoveL, thẳng lên)
    movel(p_above_place, a=1.2, v=0.25, r=0.02)

    # Fast move to next pick (MoveJ)
    movej(q_above_pick, a=1.4, v=1.05, r=0.05)

Khi MoveJ thất bại

MoveJ có thể thất bại khi:

1. Joint limits: Target nằm ngoài giới hạn khớp
2. Self-collision: Cấu hình target gây va chạm nội bộ
3. Đường đi qua vùng cấm: Dù start và end OK, đường joint space có thể đi qua vùng nguy hiểm

Giải pháp: Thêm waypoints trung gian hoặc dùng motion planner (RRT/PRM).

Khi MoveL thất bại

MoveL thất bại nhiều hơn MoveJ vì:

1. Singularity: Đường Cartesian đi qua hoặc gần singularity
2. Workspace boundary: Điểm trên đường thẳng nằm ngoài workspace
3. Joint limits: IK solution vi phạm joint limits
4. IK configuration flip: Robot "nhảy" giữa hai cấu hình IK

# Ví dụ MoveL thất bại do singularity
# Wrist singularity: theta5 ≈ 0

# Cấu hình gần wrist singularity
q_near_sing = np.array([0, -np.pi/2, 0, 0, 0.01, 0])  # theta5 ≈ 0
T = ur5e.fkine(q_near_sing)

# Jacobian gần singular
J = ur5e.jacob0(q_near_sing)
m = np.sqrt(np.linalg.det(J @ J.T))
print(f"Manipulability at near-singularity: {m:.6f}")  # Very small!
print(f"Condition number: {np.linalg.cond(J):.1f}")    # Very large!

Demo thực tế: Pick-and-Place Workflow

Đây là quy trình pick-and-place hoàn chỉnh sử dụng kết hợp MoveJ và MoveL:

# URScript — Complete pick-and-place cycle
def pick_and_place():
    # === PICK PHASE ===
    # 1. MoveJ: Di chuyển nhanh đến trên vị trí gắp
    movej(q_above_pick, a=1.4, v=1.05)

    # 2. MoveL: Tiếp cận thẳng đứng xuống vị trí gắp
    movel(p_approach_pick, a=1.2, v=0.1)

    # 3. MoveL: Tiếp cận chậm, chính xác
    movel(p_pick, a=0.5, v=0.02)

    # 4. Gắp vật
    set_digital_out(0, False)  # Close gripper
    sleep(0.3)  # Đợi gripper đóng

    # 5. MoveL: Rút thẳng lên (tránh va chạm)
    movel(p_approach_pick, a=1.2, v=0.25)

    # === PLACE PHASE ===
    # 6. MoveJ: Di chuyển nhanh đến trên vị trí đặt
    movej(q_above_place, a=1.4, v=1.05)

    # 7. MoveL: Tiếp cận thẳng xuống vị trí đặt
    movel(p_approach_place, a=1.2, v=0.1)

    # 8. MoveL: Đặt chậm, chính xác
    movel(p_place, a=0.5, v=0.02)

    # 9. Thả vật
    set_digital_out(0, True)  # Open gripper
    sleep(0.3)

    # 10. MoveL: Rút thẳng lên
    movel(p_approach_place, a=1.2, v=0.25)

    # 11. MoveJ: Về home position
    movej(q_home, a=1.4, v=1.05)

Nguyên tắc vàng khi kết hợp MoveJ/MoveL

1. MoveJ cho di chuyển tự do (không quan tâm đường TCP)
2. MoveL cho tiếp cận và rời khỏi vật (cần thẳng đứng/thẳng)
3. MoveL chậm cho thao tác chính xác (gắp, đặt, lắp ráp)
4. Blend radius cho MoveJ liên tiếp (tiết kiệm cycle time)
5. Blend radius = 0 cho điểm cuối cùng (dừng chính xác)

Tài liệu tham khảo

• Universal Robots. URScript Programming Language. UR Documentation
• Universal Robots. The URScript Manual — e-Series, SW 5.x. 2023.
• Siciliano, B. et al. Robotics: Modelling, Planning and Control. Springer, 2009. DOI: 10.1007/978-1-84628-642-1
• Corke, P. Robotics, Vision and Control (3rd ed.). Springer, 2023. DOI: 10.1007/978-3-031-07262-8

Kết luận

MoveJ và MoveL là hai lệnh nền tảng trong robot programming:

• MoveJ — nhanh, an toàn, joint interpolation, TCP path cong
• MoveL — chính xác, straight-line TCP, có thể gặp singularity
• Kết hợp đúng cách sẽ tối ưu cả cycle time và độ chính xác

Trong bài tiếp theo, chúng ta sẽ khám phá MoveC — circular/arc motion cho các ứng dụng hàn cung tròn, polishing, và screw driving.

Bài viết liên quan

• Joint Space vs Cartesian Space Planning — Lý thuyết nền tảng cho MoveJ/MoveL
• Inverse Kinematics cho robot 6-DOF — IK là cốt lõi của MoveL
• ROS 2 Control Hardware Interface — Gửi trajectory đến robot thật qua ros2_control