MoveC và MoveP: cung tròn và đường quá trình

Ở bài trước về MoveJ và MoveL, ta đã tách hai kiểu chuyển động rất quan trọng: đi theo khớp để robot đến đích nhanh, và đi theo đường thẳng trong không gian TCP để đầu công tác không "lượn" tùy ý. Nhưng trong sản xuất thật, nhiều đường không phải là đoạn thẳng. Một đường keo chạy quanh góc bo, một mối hàn quanh miệng ống, một đường đánh bóng quanh mép cong, hoặc một quỹ đạo camera quét quanh chi tiết đều cần robot đi theo cung tròn hoặc đường quá trình nhiều điểm.

Bài 10 của series tập trung vào hai lệnh cổ điển trong nhóm Cartesian motion:

• MoveC: di chuyển TCP trên một cung tròn. Cung này được xác định bởi pose hiện tại, một via point và target point.
• MoveP: di chuyển theo đường quá trình qua nhiều waypoint, giữ tốc độ TCP ổn định nhất có thể, dùng blend radius để không dừng cứng ở từng điểm.

Tên lệnh lấy cảm hứng từ URScript, nhưng mục tiêu của bài là xây dựng tư duy controller độc lập. Sau bài này, bạn sẽ biết cách fit một đường tròn từ 3 điểm, tham số hóa cung, lấy mẫu theo độ dài cung, gọi IK cho từng mẫu, và viết skeleton C++ bằng Eigen cho moveC(via, to, v, a) và moveP(vector<pose>, v, blend).

1. MoveC thực sự làm gì?

Hãy gọi ba điểm theo vị trí TCP:

• P0: điểm bắt đầu, chính là TCP hiện tại.
• P1: via point, một điểm robot phải đi qua trên cung.
• P2: target point, điểm kết thúc cung.

Nếu P0, P1, P2 không thẳng hàng, ba điểm này xác định một mặt phẳng duy nhất và một đường tròn duy nhất nằm trên mặt phẳng đó. MoveC không cần bạn nhập tâm đường tròn. Controller tự suy ra tâm, bán kính và hướng cung từ ba điểm. Đây là khác biệt lớn so với nhiều API hình học trong CAD/CAM, nơi bạn có thể nhập center + radius + angle. Trong robot teach pendant, nhập via point thường dễ hơn: người vận hành dạy một điểm giữa cung và một điểm cuối cung.

Theo tài liệu URScript PolyScope X, dạng lệnh là:

movec(pose_via, pose_to, a=1.2, v=0.25, r=0, mode=0)

Ý nghĩa thực dụng:

• pose_via: điểm thứ hai trên đường tròn; ở chế độ fixed orientation, phần rotation của via thường không được dùng.
• pose_to: điểm cuối cung.
• a: gia tốc TCP, đơn vị m/s².
• v: tốc độ TCP, đơn vị m/s.
• r: blend radius tại target pose.
• mode=0: nội suy orientation từ pose hiện tại đến pose đích.
• mode=1: giữ orientation tương đối với tiếp tuyến cung tròn.

Điều quan trọng nhất: điểm đầu của đường tròn là pose hiện tại, không phải tham số đầu tiên của movec. Vì vậy trước khi gọi MoveC, chương trình thường dùng MoveJ hoặc MoveL để đưa TCP đến đúng điểm bắt đầu.

2. Điều kiện hình học trước khi fit cung tròn

MoveC dễ dùng trên teach pendant, nhưng khi tự viết controller bạn phải tự kiểm tra dữ liệu đầu vào. Có ba lỗi phổ biến:

1. Ba điểm gần như thẳng hàng. Khi đó bán kính đường tròn tiến tới vô hạn, normal của mặt phẳng gần bằng zero, thuật toán sẽ rất nhạy với noise.
2. Via point nằm quá sát start hoặc target. Robot có thể tạo cung rất nhỏ hoặc yêu cầu gia tốc hướng quá lớn.
3. Cung đi qua vùng singularity hoặc vượt joint limit. Hình học TCP hợp lệ chưa chắc IK hợp lệ.

Một controller tử tế nên trả lỗi rõ ràng, ví dụ ArcError::CollinearPoints, ArcError::RadiusTooSmall, ArcError::IKFailedAtSample. Với người mới học, hãy nhớ quy tắc: MoveC là bài toán hình học trước, bài toán IK sau, và bài toán timing cuối cùng.

Về mặt toán học, ta dùng hai vector:

u = P1 - P0
v = P2 - P0
n = u x v

Nếu ||n|| quá nhỏ, ba điểm gần thẳng hàng. Nếu ổn, n là pháp tuyến của mặt phẳng cung. Tâm đường tròn là giao của hai đường trung trực trong mặt phẳng: đường trung trực của đoạn P0-P1 và đường trung trực của đoạn P0-P2.

3. Prototype Python: fit đường tròn từ 3 điểm

Prototype Python dưới đây cố tình không phụ thuộc ROS hay robot thật. Nó chỉ làm ba việc: fit đường tròn, chọn cung đi qua via point, và vẽ kết quả. Phần IK được để thành hàm giả để bạn thay bằng IK đã viết trong bài FK/IK/Jacobian hoặc thư viện riêng.

import numpy as np
import matplotlib.pyplot as plt

EPS = 1e-9

def normalize(x):
    n = np.linalg.norm(x)
    if n < EPS:
        raise ValueError("Vector quá nhỏ để chuẩn hóa")
    return x / n

def circle_from_3_points(p0, p1, p2):
    p0 = np.asarray(p0, dtype=float)
    p1 = np.asarray(p1, dtype=float)
    p2 = np.asarray(p2, dtype=float)

    a = p1 - p0
    b = p2 - p0
    normal = np.cross(a, b)
    normal_norm = np.linalg.norm(normal)
    if normal_norm < 1e-6:
        raise ValueError("Ba điểm gần thẳng hàng, không fit được cung tròn ổn định")
    normal = normal / normal_norm

    m01 = 0.5 * (p0 + p1)
    m02 = 0.5 * (p0 + p2)

    d01 = normalize(np.cross(normal, a))
    d02 = normalize(np.cross(normal, b))

    # Tìm s, t sao cho m01 + s*d01 gần bằng m02 + t*d02.
    A = np.column_stack([d01, -d02])
    rhs = m02 - m01
    s, t = np.linalg.lstsq(A, rhs, rcond=None)[0]
    center = m01 + s * d01
    radius = np.linalg.norm(p0 - center)
    return center, radius, normal

def angle_on_plane(point, center, e0, e1):
    r = point - center
    return np.arctan2(np.dot(r, e1), np.dot(r, e0))

def wrap_positive(theta):
    while theta < 0:
        theta += 2 * np.pi
    while theta >= 2 * np.pi:
        theta -= 2 * np.pi
    return theta

def arc_angles_through_via(p0, p1, p2, center, normal):
    e0 = normalize(p0 - center)
    e1 = normalize(np.cross(normal, e0))

    theta_via = wrap_positive(angle_on_plane(p1, center, e0, e1))
    theta_end = wrap_positive(angle_on_plane(p2, center, e0, e1))

    # Cung dương từ 0 đến theta_end có chứa via không?
    if 0.0 <= theta_via <= theta_end:
        return 0.0, theta_end, e0, e1

    # Nếu không, đi theo chiều âm. Đại diện bằng góc end âm.
    if theta_end > 0:
        theta_end -= 2 * np.pi
    return 0.0, theta_end, e0, e1

def sample_movec(p0, p1, p2, ds=0.005):
    center, radius, normal = circle_from_3_points(p0, p1, p2)
    theta0, theta2, e0, e1 = arc_angles_through_via(p0, p1, p2, center, normal)

    arc_len = abs(theta2 - theta0) * radius
    n = max(2, int(np.ceil(arc_len / ds)) + 1)
    thetas = np.linspace(theta0, theta2, n)

    points = np.array([
        center + radius * (np.cos(t) * e0 + np.sin(t) * e1)
        for t in thetas
    ])
    return points, center, radius, normal

def ik_solve_stub(tcp_position, q_seed):
    # Thay bằng IK thật: analytic IK, DLS Jacobian, TRAC-IK, Pinocchio, v.v.
    # Ở đây ta chỉ trả seed để minh họa pipeline.
    return q_seed

p0 = np.array([0.45, -0.15, 0.32])
p1 = np.array([0.50,  0.00, 0.36])
p2 = np.array([0.42,  0.16, 0.32])

points, center, radius, normal = sample_movec(p0, p1, p2, ds=0.004)
q = np.zeros(6)
joint_path = []
for p in points:
    q = ik_solve_stub(p, q)
    joint_path.append(q)

fig = plt.figure(figsize=(9, 6))
ax = fig.add_subplot(111, projection="3d")
ax.plot(points[:, 0], points[:, 1], points[:, 2], label="MoveC arc")
ax.scatter(*p0, label="start P0", s=60)
ax.scatter(*p1, label="via P1", s=60)
ax.scatter(*p2, label="target P2", s=60)
ax.scatter(*center, label=f"center, r={radius:.3f} m", s=60)
ax.set_xlabel("x [m]")
ax.set_ylabel("y [m]")
ax.set_zlabel("z [m]")
ax.legend()
plt.tight_layout()
plt.show()

Điểm cần chú ý là đoạn chọn chiều cung. Có hai cung nối P0 và P2: cung ngắn và cung dài. Via point nói cho controller biết phải đi theo cung nào. Với robot công nghiệp, nhiều hệ thống còn giới hạn cung không được là full circle trong một lệnh, hoặc yêu cầu dùng hai lệnh MoveC để tạo vòng tròn đầy đủ. Lý do là nếu start và end trùng nhau, chỉ một via point không đủ để xác định duy nhất hướng và số vòng.

4. Tốc độ TCP và lấy mẫu theo độ dài cung

Nếu bạn lấy mẫu theo góc đều, khoảng cách giữa hai điểm liên tiếp trên cùng một cung tròn là gần đều vì ds = r * dtheta. Với MoveC đơn giản, cách này đủ tốt. Timing sau đó có thể dùng cùng tư duy ở bài trajectory types: tạo danh sách waypoint trong Cartesian, giải IK thành danh sách joint waypoint, rồi gán timestamp bằng profile vận tốc.

Tuy vậy, có một bẫy: tốc độ TCP hằng không tự động đảm bảo tốc độ từng khớp nằm trong giới hạn. Khi cung đi gần singularity, một thay đổi TCP nhỏ có thể tạo thay đổi joint lớn. Vì vậy pipeline thực tế nên có bước retime hoặc giảm v nếu bất kỳ đoạn nào vượt qdot_max, qddot_max.

Ảnh trên là profile vận tốc cơ bản. Với Cartesian arc, ý tưởng tương tự: tăng tốc từ 0 lên v, chạy gần hằng tốc ở giữa, rồi giảm về 0 nếu không blend sang đoạn kế tiếp. Nếu có blend radius, robot không bắt buộc dừng ở target; nó có thể rời cung trước điểm chính xác một chút để nối mượt sang đoạn sau.

5. MoveP: đường quá trình nhiều điểm

MoveP thường dùng cho quá trình cần tốc độ TCP ổn định: bơm keo, phun sơn, hàn, đánh bóng, phun chất phủ. Theo tài liệu Universal Robots, MoveP là chuyển động process: robot đi tuyến tính trong tool-space và dùng circular blends, với mục tiêu giữ constant tool speed. Blend radius càng nhỏ thì góc càng sắc; radius lớn hơn thì đường càng mượt nhưng robot đi xa waypoint danh nghĩa hơn.

Hãy tưởng tượng bạn có đường đi:

P0 -> P1 -> P2 -> P3 -> P4

Nếu dùng MoveL với r=0, robot sẽ giảm tốc và dừng tại P1, P2, P3. Với bơm keo, dừng như vậy làm keo tụ lại thành cục. MoveP dùng blend quanh các góc: trước khi đến P1, robot rời đoạn thẳng P0-P1, đi qua một cung tròn nhỏ, rồi nhập vào đoạn P1-P2. TCP không nhất thiết chạm chính xác P1; nó đi trong một vùng bán kính blend quanh P1.

Quy tắc an toàn cho beginner: blend phải nhỏ hơn nửa chiều dài của hai đoạn kề ngắn nhất. Nếu không, hai vùng blend chồng lên nhau. Tài liệu MoveIt Pro cũng nêu nguyên tắc tương tự cho Cartesian path blending: blending radius phải đủ nhỏ để áp dụng được tại từng waypoint.

6. C++ Eigen: skeleton Pose và MoveC

Đoạn C++ dưới đây là một file main.cpp tối giản. Nó không điều khiển robot thật, nhưng tạo được danh sách pose mẫu cho MoveC và MoveP. Eigen được dùng vì Vector3d, cross(), dot(), normalized() rất hợp với hình học 3D.

#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <stdexcept>
#include <vector>

using Vec3 = Eigen::Vector3d;

struct Pose {
  Vec3 p;
  Eigen::Quaterniond q;
};

struct Arc {
  Vec3 center;
  Vec3 normal;
  double radius;
};

static Vec3 normalizedChecked(const Vec3& v, const char* name) {
  const double n = v.norm();
  if (n < 1e-9) {
    throw std::runtime_error(std::string(name) + " is too small");
  }
  return v / n;
}

static Arc circleFrom3Points(const Vec3& p0, const Vec3& p1, const Vec3& p2) {
  const Vec3 a = p1 - p0;
  const Vec3 b = p2 - p0;
  Vec3 normal = a.cross(b);
  if (normal.norm() < 1e-6) {
    throw std::runtime_error("MoveC points are nearly collinear");
  }
  normal.normalize();

  const Vec3 m01 = 0.5 * (p0 + p1);
  const Vec3 m02 = 0.5 * (p0 + p2);
  const Vec3 d01 = normalizedChecked(normal.cross(a), "d01");
  const Vec3 d02 = normalizedChecked(normal.cross(b), "d02");

  Eigen::Matrix<double, 3, 2> A;
  A.col(0) = d01;
  A.col(1) = -d02;
  Eigen::Vector2d st = A.colPivHouseholderQr().solve(m02 - m01);

  Vec3 center = m01 + st(0) * d01;
  double radius = (p0 - center).norm();
  return {center, normal, radius};
}

static double wrapPositive(double x) {
  const double twoPi = 2.0 * M_PI;
  while (x < 0.0) x += twoPi;
  while (x >= twoPi) x -= twoPi;
  return x;
}

static double angleOnPlane(const Vec3& p, const Vec3& c, const Vec3& e0, const Vec3& e1) {
  Vec3 r = p - c;
  return std::atan2(r.dot(e1), r.dot(e0));
}

std::vector<Pose> moveC(const Pose& start, const Pose& via, const Pose& to,
                        double v, double a, double ds = 0.005) {
  (void)v;
  (void)a;
  Arc arc = circleFrom3Points(start.p, via.p, to.p);
  Vec3 e0 = normalizedChecked(start.p - arc.center, "arc e0");
  Vec3 e1 = normalizedChecked(arc.normal.cross(e0), "arc e1");

  double thetaVia = wrapPositive(angleOnPlane(via.p, arc.center, e0, e1));
  double thetaTo = wrapPositive(angleOnPlane(to.p, arc.center, e0, e1));
  if (!(0.0 <= thetaVia && thetaVia <= thetaTo)) {
    thetaTo -= 2.0 * M_PI;
  }

  double length = std::abs(thetaTo) * arc.radius;
  int n = std::max(2, static_cast<int>(std::ceil(length / ds)) + 1);

  std::vector<Pose> out;
  out.reserve(n);
  for (int i = 0; i < n; ++i) {
    double s = static_cast<double>(i) / static_cast<double>(n - 1);
    double theta = s * thetaTo;
    Pose pose;
    pose.p = arc.center + arc.radius * (std::cos(theta) * e0 + std::sin(theta) * e1);
    pose.q = start.q.slerp(s, to.q);
    out.push_back(pose);
  }
  return out;
}

Ở đây v và a chưa được dùng để timestamp, vì ta đang tập trung vào hình học. Trong controller thật, kết quả std::vector<Pose> sẽ đi qua IK, collision check, joint limit check, rồi time parameterization. Cách tách tầng như vậy giúp code dễ debug: nếu đường TCP sai, lỗi nằm ở geometry; nếu TCP đúng nhưng robot giật, lỗi nằm ở IK hoặc retiming.

7. C++ Eigen: MoveP nhiều điểm với blend radius

MoveP đầy đủ có thể rất phức tạp, nhất là khi phải blend orientation, tránh collision và xử lý đoạn ngắn. Bản beginner dưới đây dùng ý tưởng đơn giản:

• Đi thẳng trên từng đoạn.
• Tại waypoint giữa, cắt lùi một đoạn blend trên đoạn trước và cắt tiến một đoạn blend trên đoạn sau.
• Nối hai điểm cắt bằng một cung tròn qua chính waypoint đó.

static std::vector<Pose> sampleLine(const Pose& a, const Pose& b, double ds) {
  double length = (b.p - a.p).norm();
  int n = std::max(2, static_cast<int>(std::ceil(length / ds)) + 1);
  std::vector<Pose> out;
  for (int i = 0; i < n; ++i) {
    double s = static_cast<double>(i) / static_cast<double>(n - 1);
    Pose p;
    p.p = (1.0 - s) * a.p + s * b.p;
    p.q = a.q.slerp(s, b.q);
    out.push_back(p);
  }
  return out;
}

std::vector<Pose> moveP(const std::vector<Pose>& waypoints, double v,
                        double blend, double ds = 0.005) {
  if (waypoints.size() < 2) {
    throw std::runtime_error("MoveP needs at least two waypoints");
  }

  std::vector<Pose> path;
  Pose current = waypoints.front();

  for (size_t i = 1; i + 1 < waypoints.size(); ++i) {
    const Pose& prev = waypoints[i - 1];
    const Pose& corner = waypoints[i];
    const Pose& next = waypoints[i + 1];

    Vec3 dirIn = normalizedChecked(corner.p - prev.p, "MoveP dirIn");
    Vec3 dirOut = normalizedChecked(next.p - corner.p, "MoveP dirOut");
    double maxBlend = 0.45 * std::min((corner.p - prev.p).norm(), (next.p - corner.p).norm());
    double r = std::min(blend, maxBlend);

    Pose cutIn{corner.p - r * dirIn, corner.q};
    Pose cutOut{corner.p + r * dirOut, corner.q};

    auto line = sampleLine(current, cutIn, ds);
    path.insert(path.end(), line.begin(), line.end());

    auto arc = moveC(cutIn, corner, cutOut, v, /*a=*/1.0, ds);
    path.insert(path.end(), arc.begin(), arc.end());
    current = cutOut;
  }

  auto last = sampleLine(current, waypoints.back(), ds);
  path.insert(path.end(), last.begin(), last.end());
  return path;
}

int main() {
  Eigen::Quaterniond q = Eigen::Quaterniond::Identity();
  Pose p0{{0.35, -0.20, 0.30}, q};
  Pose p1{{0.45, -0.05, 0.34}, q};
  Pose p2{{0.42,  0.15, 0.34}, q};
  Pose p3{{0.30,  0.22, 0.31}, q};

  auto arc = moveC(p0, p1, p2, 0.10, 0.50);
  auto process = moveP({p0, p1, p2, p3}, 0.08, 0.03);

  std::cout << "MoveC samples: " << arc.size() << "\n";
  std::cout << "MoveP samples: " << process.size() << "\n";
  return 0;
}

CMakeLists.txt tối thiểu:

cmake_minimum_required(VERSION 3.16)
project(classical_arm_paths LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(Eigen3 3.4 REQUIRED NO_MODULE)

add_executable(paths main.cpp)
target_link_libraries(paths PRIVATE Eigen3::Eigen)

Build:

mkdir -p build
cmake -S . -B build
cmake --build build
./build/paths

8. Từ mẫu TCP sang lệnh joint

Sau khi có các pose TCP, pipeline cơ bản là:

1. Với mỗi pose, giải IK dùng seed là nghiệm của pose trước.
2. Nếu IK fail, giảm bước ds, đổi branch IK, hoặc báo path không khả thi.
3. Kiểm tra joint limit và self-collision.
4. Gán timestamp bằng profile vận tốc.
5. Gửi JointTrajectory xuống servo controller.

MoveIt tutorial về Cartesian path cũng dùng tư duy tương tự: người dùng đưa danh sách waypoint cho end-effector, hệ thống nội suy Cartesian path theo eef_step, rồi tạo trajectory. Khi làm controller riêng, bạn không cần bê nguyên MoveIt vào bài toán nhỏ, nhưng nên học cách họ tách waypoint, IK, path validation và execution.

9. Checklist debug khi đường cung sai

Nếu cung tròn vẽ ra không như mong muốn, hãy kiểm tra theo thứ tự:

• P0, P1, P2 có đúng frame không? Lỗi nhầm base frame/tool frame rất phổ biến; xem lại bài frames và jogging.
• Ba điểm có gần thẳng hàng không?
• Via point có nằm trên cung bạn muốn không, hay bạn dạy nhầm phía?
• Đơn vị là mét hay milimét?
• Orientation đang slerp theo target hay giữ tương đối với tangent?
• IK có nhảy branch giữa hai mẫu liên tiếp không?
• Blend radius có quá lớn so với khoảng cách giữa các waypoint không? Xem thêm bài blending để hiểu vùng blend và lỗi overlapping blends.

Với process path, lỗi hay gặp nhất là chọn blend quá lớn. Robot nhìn rất mượt nhưng cắt góc quá nhiều, làm đường keo hoặc đường hàn lệch khỏi chi tiết. Hãy bắt đầu với radius nhỏ, vẽ đường TCP, đo sai lệch tối đa, rồi mới tăng radius để giảm rung và giữ tốc độ.

10. Bài tập tự làm

1. Sửa prototype Python để xuất points.csv, mỗi dòng là x,y,z.
2. Thay ik_solve_stub() bằng IK DLS Jacobian từ bài trước.
3. Vẽ đồ thị ||q[i+1] - q[i]|| để phát hiện joint jump.
4. Tạo một MoveP hình chữ U với 5 waypoint và thử blend = 0, 0.01, 0.03.
5. Thêm giới hạn max_tcp_deviation để cảnh báo khi blend cắt góc quá nhiều.

Nếu muốn đưa vào production, bước tiếp theo là tích hợp path này với collision checking và motion planning. Bạn có thể đọc thêm Open-RMF fleet management để thấy cách hệ thống lớn tách planning, execution và monitoring, hoặc SLAM navigation robot để so sánh path planning của mobile robot với Cartesian path của robot arm.

Nguồn tham khảo

• Universal Robots, URScript movec(pose_via, pose_to, a, v, r, mode) và movep(pose, a, v, r): https://www.universal-robots.com/manuals/EN/PDF/SW10_7/scriptmanual_psx/script_directory_PolyscopeX.pdf
• Universal Robots, Circular path using MoveP/MoveC: https://www.universal-robots.com/articles/ur/programming/circular-path-using-movepmovec/
• Universal Robots PolyScope Move node, MoveP process operation: https://www.universal-robots.com/manuals/EN/HTML/SW5_25/Content/prod-usr-man/software/PolyScope/content/BasicProgNodes/commandtab_move_en.htm
• MoveIt Tutorials, Cartesian path và RViz visualization: https://moveit.github.io/moveit_tutorials/
• MoveIt Pro Cartesian Path Following, blending radius constraint: https://docs.picknik.ai/6/how_to/cartesian_path_following/
• Eigen documentation, vector arithmetic và CMake imported target: https://libeigen.gitlab.io/eigen/docs-nightly/group__TutorialMatrixArithmetic.html

Bài viết liên quan

• MoveJ và MoveL: chuyển động theo khớp và đường thẳng
• Các kiểu trajectory trong robot arm
• Blending: đi qua waypoint mà không dừng cứng