MoveC và Circular Motion: Arc, Helix và Orbital Paths

Sau khi thành thạo MoveJ và MoveL, bạn đã có thể lập trình robot di chuyển theo đường thẳng và di chuyển tự do. Nhưng thế giới thực không chỉ có đường thẳng — nhiều ứng dụng công nghiệp yêu cầu chuyển động cung tròn: hàn mối cong, đánh bóng bề mặt tròn, vặn vít (screw driving), kiểm tra orbital. Đó là lúc MoveC xuất hiện.

MoveC — Circular Arc Motion

Nguyên lý hoạt động

MoveC tạo chuyển động theo cung tròn (arc) qua 3 điểm:

1. Start point ($P_0$): Vị trí hiện tại của TCP
2. Via point ($P_1$): Điểm trung gian trên cung tròn
3. End point ($P_2$): Điểm đích

Ba điểm xác định duy nhất một mặt phẳng và một đường tròn. Controller tính:

• Tâm đường tròn từ 3 điểm
• Bán kính cung tròn
• Mặt phẳng chứa cung tròn
• Chia cung tròn thành nhiều interpolation points
• Tại mỗi điểm: giải IK (giống MoveL)

Thuật toán tìm tâm đường tròn qua 3 điểm

import numpy as np

def circle_from_3_points(p0, p1, p2):
    """
    Tìm tâm và bán kính đường tròn qua 3 điểm trong 3D.

    Returns:
        center: tâm đường tròn [x, y, z]
        radius: bán kính
        normal: vector pháp tuyến mặt phẳng
    """
    # Vectors từ p0
    v1 = p1 - p0
    v2 = p2 - p0

    # Normal vector (pháp tuyến mặt phẳng)
    normal = np.cross(v1, v2)
    normal = normal / np.linalg.norm(normal)

    # Midpoints
    m1 = (p0 + p1) / 2
    m2 = (p0 + p2) / 2

    # Perpendicular bisectors trong mặt phẳng
    d1 = np.cross(v1, normal)
    d2 = np.cross(v2, normal)

    # Giải hệ phương trình tìm tâm
    # m1 + s*d1 = m2 + t*d2 (trên mặt phẳng)
    # Dùng least squares
    A = np.column_stack([d1, -d2])
    b = m2 - m1
    params = np.linalg.lstsq(A, b, rcond=None)[0]

    center = m1 + params[0] * d1
    radius = np.linalg.norm(center - p0)

    return center, radius, normal

# Test với 3 điểm
p0 = np.array([0.4, 0.0, 0.3])
p1 = np.array([0.3, 0.1, 0.3])   # Via point
p2 = np.array([0.2, 0.0, 0.3])   # End point

center, radius, normal = circle_from_3_points(p0, p1, p2)
print(f"Center: {center}")
print(f"Radius: {radius:.4f} m")
print(f"Normal: {normal}")

URScript MoveC

# URScript
# movec(pose_via, pose_to, a=1.2, v=0.25, r=0, mode=0)
# pose_via: via point (trung gian)
# pose_to: end point (đích)
# a: acceleration [m/s²]
# v: velocity [m/s]
# r: blend radius [m]
# mode: 0 = unconstrained, 1 = fixed orientation

# Cung tròn đơn giản
movec(p[0.3, 0.1, 0.3, 0, 3.14, 0],    # Via point
      p[0.2, 0.0, 0.3, 0, 3.14, 0],    # End point
      a=1.2, v=0.1)

# Full circle (via = điểm đối diện qua tâm)
# Start: [0.4, 0, 0.3] → Via: [0.2, 0, 0.3] → End: [0.4, 0, 0.3]
movec(p[0.2, 0.0, 0.3, 0, 3.14, 0],
      p[0.4, 0.0, 0.3, 0, 3.14, 0],
      a=1.2, v=0.05)

Python Implementation

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def generate_arc_trajectory(p0, p1, p2, n_points=100):
    """Generate circular arc trajectory through 3 points."""
    center, radius, normal = circle_from_3_points(p0, p1, p2)

    # Tính góc cho mỗi điểm trên cung
    v_start = p0 - center
    v_end = p2 - center

    # Hệ tọa độ cục bộ trên mặt phẳng tròn
    u = v_start / np.linalg.norm(v_start)
    w = np.cross(normal, u)

    # Góc start và end
    theta_start = np.arctan2(np.dot(v_start, w), np.dot(v_start, u))
    theta_end = np.arctan2(np.dot(v_end, w), np.dot(v_end, u))

    # Kiểm tra via point để xác định hướng quay
    v_via = p1 - center
    theta_via = np.arctan2(np.dot(v_via, w), np.dot(v_via, u))

    # Adjust angles cho hướng quay đúng
    if theta_end < theta_start:
        if theta_via > theta_start or theta_via < theta_end:
            pass  # OK — quay theo chiều dương
        else:
            theta_end += 2 * np.pi
    else:
        if theta_start < theta_via < theta_end:
            pass
        else:
            theta_end -= 2 * np.pi

    # Generate points
    thetas = np.linspace(theta_start, theta_end, n_points)
    positions = []
    for theta in thetas:
        point = center + radius * (np.cos(theta) * u + np.sin(theta) * w)
        positions.append(point)

    return np.array(positions), center, radius

# Generate arc
p0 = np.array([0.4, 0.0, 0.3])
p1 = np.array([0.3, 0.1, 0.3])
p2 = np.array([0.2, 0.0, 0.3])

arc_points, center, radius = generate_arc_trajectory(p0, p1, p2)

# Visualize
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
ax.plot(arc_points[:, 0], arc_points[:, 1], arc_points[:, 2],
        'b-', linewidth=2, label='Arc path')
ax.scatter(*p0, c='green', s=100, zorder=5, label='Start')
ax.scatter(*p1, c='orange', s=100, zorder=5, label='Via')
ax.scatter(*p2, c='red', s=100, zorder=5, label='End')
ax.scatter(*center, c='purple', s=80, marker='+', zorder=5, label='Center')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title(f'MoveC Arc Trajectory (r={radius:.3f}m)')
ax.legend()
plt.tight_layout()
plt.savefig('movec_arc.png', dpi=150)
plt.show()

Full Circle vs Partial Arc

Partial Arc (Phổ biến nhất)

Cung tròn một phần — ví dụ hàn 1/4 hoặc 1/2 vòng tròn:

# URScript — hàn cung tròn 90 độ
movel(p_start_weld, a=0.5, v=0.01)  # Đến điểm bắt đầu hàn
set_digital_out(1, True)            # Bật mỏ hàn
movec(p_via_weld, p_end_weld, a=0.3, v=0.005)  # Hàn theo cung
set_digital_out(1, False)           # Tắt mỏ hàn
movel(p_retreat, a=1.2, v=0.25)     # Rút mỏ hàn

Full Circle

Để tạo vòng tròn đầy đủ, end point = start point, via point = điểm đối diện:

# Full circle trong Python
def generate_full_circle(center, radius, normal, n_points=200):
    """Generate full circle trajectory."""
    u = np.array([1, 0, 0])
    if abs(np.dot(u, normal)) > 0.9:
        u = np.array([0, 1, 0])
    u = u - np.dot(u, normal) * normal
    u = u / np.linalg.norm(u)
    w = np.cross(normal, u)

    thetas = np.linspace(0, 2*np.pi, n_points, endpoint=False)
    positions = []
    for theta in thetas:
        point = center + radius * (np.cos(theta) * u + np.sin(theta) * w)
        positions.append(point)

    return np.array(positions)

# Vòng tròn bán kính 0.1m, tâm tại [0.3, 0, 0.3]
circle_path = generate_full_circle(
    center=np.array([0.3, 0.0, 0.3]),
    radius=0.1,
    normal=np.array([0, 0, 1])  # Mặt phẳng XY
)

Helix — Cung tròn + Chuyển động tuyến tính

Helix (xoắn ốc) kết hợp chuyển động tròn với di chuyển tuyến tính theo trục Z. Ứng dụng:

• Vặn vít (screw driving): Xoay + đẩy xuống
• Đánh bóng trụ tròn: Xoay quanh trục + di chuyển dọc
• Khoan: Xoay + tiến vào

def generate_helix(center, radius, normal, pitch, n_turns, n_points=500):
    """
    Generate helix trajectory.

    Args:
        center: start center position
        radius: helix radius
        normal: axis direction (unit vector)
        pitch: distance traveled along axis per revolution
        n_turns: number of turns
        n_points: number of trajectory points
    """
    # Hệ tọa độ cục bộ
    u = np.array([1, 0, 0])
    if abs(np.dot(u, normal)) > 0.9:
        u = np.array([0, 1, 0])
    u = u - np.dot(u, normal) * normal
    u = u / np.linalg.norm(u)
    w = np.cross(normal, u)

    total_angle = 2 * np.pi * n_turns
    thetas = np.linspace(0, total_angle, n_points)

    positions = []
    for theta in thetas:
        # Circular component
        circle = radius * (np.cos(theta) * u + np.sin(theta) * w)
        # Linear component along axis
        linear = (theta / (2 * np.pi)) * pitch * normal
        positions.append(center + circle + linear)

    return np.array(positions)

# Helix cho screw driving
helix_path = generate_helix(
    center=np.array([0.3, 0.2, 0.3]),
    radius=0.005,    # 5mm radius (nhỏ — mô phỏng vặn vít)
    normal=np.array([0, 0, -1]),  # Hướng xuống
    pitch=0.001,     # 1mm mỗi vòng (thread pitch)
    n_turns=5
)

# Helix cho polishing
polishing_helix = generate_helix(
    center=np.array([0.3, 0.0, 0.3]),
    radius=0.05,     # 50mm radius
    normal=np.array([0, 0, 1]),
    pitch=0.02,      # 20mm mỗi vòng
    n_turns=3
)

# Visualize
fig = plt.figure(figsize=(14, 6))

ax1 = fig.add_subplot(121, projection='3d')
ax1.plot(helix_path[:, 0], helix_path[:, 1], helix_path[:, 2],
         'b-', linewidth=1.5)
ax1.set_title('Screw Driving Helix')
ax1.set_xlabel('X'); ax1.set_ylabel('Y'); ax1.set_zlabel('Z')

ax2 = fig.add_subplot(122, projection='3d')
ax2.plot(polishing_helix[:, 0], polishing_helix[:, 1], polishing_helix[:, 2],
         'r-', linewidth=1.5)
ax2.set_title('Polishing Helix')
ax2.set_xlabel('X'); ax2.set_ylabel('Y'); ax2.set_zlabel('Z')

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

URScript Helix (xấp xỉ bằng chuỗi MoveC)

# URScript — helix bằng chuỗi movec
# Mỗi movec là 1 nửa vòng, thêm offset Z giữa các lệnh
import math

def screw_drive(center_x, center_y, z_start, radius, pitch, n_turns):
    """Generate URScript for screw driving helix."""
    steps_per_turn = 4  # 4 movec per turn (mỗi movec = 90 độ)
    total_steps = int(n_turns * steps_per_turn)

    for i in range(total_steps):
        angle_via = (i + 0.5) * (2 * math.pi / steps_per_turn)
        angle_end = (i + 1) * (2 * math.pi / steps_per_turn)

        z_via = z_start - (i + 0.5) * pitch / steps_per_turn
        z_end = z_start - (i + 1) * pitch / steps_per_turn

        via_x = center_x + radius * math.cos(angle_via)
        via_y = center_y + radius * math.sin(angle_via)
        end_x = center_x + radius * math.cos(angle_end)
        end_y = center_y + radius * math.sin(angle_end)

        # movec(via, end, ...)
        print(f"movec(p[{via_x:.4f}, {via_y:.4f}, {z_via:.4f}, 0, 3.14, 0], "
              f"p[{end_x:.4f}, {end_y:.4f}, {z_end:.4f}, 0, 3.14, 0], "
              f"a=0.5, v=0.01)")

screw_drive(0.3, 0.2, 0.3, 0.005, 0.001, 3)

Orbital Paths — Kiểm tra và Scanning

Orbital path — robot di chuyển trên quỹ đạo tròn quanh một vật thể, giữ tool hướng vào tâm. Ứng dụng:

• Kiểm tra chất lượng: Camera quay quanh sản phẩm
• 3D scanning: LiDAR/camera 360 độ
• Phun sơn trụ tròn: Giữ khoảng cách đều

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

def generate_orbital_path(object_center, orbit_radius, orbit_height,
                          n_points=100, n_layers=3, layer_spacing=0.05):
    """
    Generate orbital inspection path.
    Robot orbits around object, looking inward.
    """
    positions = []
    orientations = []

    for layer in range(n_layers):
        z = orbit_height + layer * layer_spacing
        thetas = np.linspace(0, 2*np.pi, n_points, endpoint=False)

        for theta in thetas:
            # Position on orbit
            x = object_center[0] + orbit_radius * np.cos(theta)
            y = object_center[1] + orbit_radius * np.sin(theta)
            positions.append([x, y, z])

            # Orientation: tool points toward center
            look_dir = object_center[:2] - np.array([x, y])
            yaw = np.arctan2(look_dir[1], look_dir[0])
            orientations.append(yaw)

    return np.array(positions), orientations

# Orbital inspection path
orbit_pos, orbit_orient = generate_orbital_path(
    object_center=np.array([0.3, 0.0, 0.2]),
    orbit_radius=0.15,
    orbit_height=0.15,
    n_points=50,
    n_layers=3,
    layer_spacing=0.04
)

fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
ax.plot(orbit_pos[:, 0], orbit_pos[:, 1], orbit_pos[:, 2],
        'b-', linewidth=1, alpha=0.7)
ax.scatter(0.3, 0.0, 0.2, c='red', s=200, marker='*', label='Object')
ax.set_xlabel('X (m)')
ax.set_ylabel('Y (m)')
ax.set_zlabel('Z (m)')
ax.set_title('Orbital Inspection Path (3 layers)')
ax.legend()
plt.tight_layout()
plt.savefig('orbital_path.png', dpi=150)
plt.show()

Common Pitfalls — Cạm bẫy thường gặp

1. Collinear Points (3 điểm thẳng hàng)

Nếu 3 điểm nằm trên một đường thẳng, không thể xác định đường tròn — MoveC sẽ lỗi.

# KIỂM TRA collinearity trước khi gọi movec
def check_collinear(p0, p1, p2, threshold=1e-6):
    """Check if 3 points are collinear."""
    v1 = p1 - p0
    v2 = p2 - p0
    cross = np.linalg.norm(np.cross(v1, v2))
    return cross < threshold

# BAD — 3 điểm thẳng hàng
p0 = np.array([0.2, 0.0, 0.3])
p1 = np.array([0.3, 0.0, 0.3])
p2 = np.array([0.4, 0.0, 0.3])
print(f"Collinear: {check_collinear(p0, p1, p2)}")  # True — ERROR!

2. Orientation Flipping

Khi di chuyển theo cung tròn, orientation có thể "nhảy" đột ngột — đặc biệt khi cung tròn lớn hơn 180 độ.

Giải pháp: Dùng mode=1 (fixed orientation) trong URScript hoặc chia cung tròn thành nhiều đoạn nhỏ.

3. Bán kính quá lớn/nhỏ

• Bán kính quá lớn → Cung tròn gần đường thẳng → Dùng MoveL thay vì MoveC
• Bán kính quá nhỏ → Gia tốc ly tâm lớn → Robot chậm lại hoặc lỗi

Ứng dụng thực tế

Hàn mối tròn (Circular Weld Seam)

# URScript — hàn mối tròn
def circular_weld(center, radius, weld_speed=0.005):
    """Weld a circular seam."""
    import math

    # Tiếp cận vị trí bắt đầu
    start_x = center[0] + radius
    movel(p[start_x, center[1], center[2]+0.05, 0, 3.14, 0],
          a=1.2, v=0.25)
    movel(p[start_x, center[1], center[2], 0, 3.14, 0],
          a=0.5, v=0.05)

    # Bật mỏ hàn
    set_digital_out(1, True)
    sleep(0.5)  # Ổn định hồ quang

    # Hàn full circle (2 movec cho 1 vòng)
    movec(
        p[center[0], center[1]+radius, center[2], 0, 3.14, 0],
        p[center[0]-radius, center[1], center[2], 0, 3.14, 0],
        a=0.3, v=weld_speed
    )
    movec(
        p[center[0], center[1]-radius, center[2], 0, 3.14, 0],
        p[start_x, center[1], center[2], 0, 3.14, 0],
        a=0.3, v=weld_speed
    )

    # Tắt mỏ hàn
    set_digital_out(1, False)

    # Rút mỏ hàn
    movel(p[start_x, center[1], center[2]+0.05, 0, 3.14, 0],
          a=1.2, v=0.25)

Tài liệu tham khảo

• Universal Robots. URScript Programming Language — movec function. UR Documentation
• Gasparetto, A. et al. "Path Planning and Trajectory Planning Algorithms." Springer, 2015. DOI: 10.1007/978-3-319-14705-5_1
• Lynch, K.M., Park, F.C. Modern Robotics: Mechanics, Planning, and Control. Cambridge, 2017. Link

Kết luận

MoveC mở rộng khả năng lập trình robot vượt xa đường thẳng:

• Arc — cung tròn qua 3 điểm cho hàn, cắt
• Helix — xoắn ốc cho vặn vít, đánh bóng trụ
• Orbital — quỹ đạo kiểm tra quanh vật

Trong bài tiếp theo, chúng ta sẽ học cách blend các motion segments — kỹ thuật quan trọng nhất để tối ưu cycle time mà vẫn giữ chuyển động mượt.

Bài viết liên quan

• MoveJ và MoveL chi tiết — Hai lệnh cơ bản trước khi học MoveC
• Grasping và Manipulation cơ bản — Kỹ thuật gắp vật kết hợp MoveL và MoveC
• ROS 2 Control Hardware Interface — Gửi circular trajectory qua ros2_control