MoveL: chuyển động thẳng Cartesian với IK

Trong bài trước, ta đã có nền tảng để hiểu sự khác nhau giữa MoveJ và MoveL: MoveJ nội suy trong joint space, còn MoveL cố giữ TCP đi theo đường thẳng trong Cartesian space. Bài này đi vào phần mà controller cổ điển thật sự phải làm: từ pose hiện tại T0 và pose đích T1, tạo một chuỗi waypoint Cartesian nằm trên đoạn thẳng, nội suy orientation bằng quaternion SLERP, gọi IK ở từng waypoint, rồi kiểm tra lại bằng FK xem TCP có còn thẳng hay không.

Nếu bạn mới theo series, nên đọc lại mô hình robot và FK, inverse kinematics, và MoveJ, MoveL trong lập trình công nghiệp. Bài ngoài series hữu ích là Simulation cho Robotics: MuJoCo vs Isaac Sim vs Gazebo, vì MoveL nên được thử trong simulator trước khi đưa vào robot thật.

MoveL thực sự cam kết điều gì?

Trong tài liệu kiểu Universal Robots, MoveL được mô tả là lệnh mà tool/TCP di chuyển theo đường thẳng; để giữ TCP tuyến tính, từng khớp phải đi theo chuyển động phức tạp hơn so với một phép nội suy joint đơn giản. MoveIt cũng diễn đạt tương tự: khi bật Cartesian path, end-effector cố đi tuyến tính trong Cartesian space. Các bài toán công nghiệp như hàn, phun sơn, bôi keo, mài cạnh, đưa đầu hút xuống theo phương Z đều cần cam kết này.

Điểm dễ nhầm là MoveL không có nghĩa mọi khớp di chuyển tuyến tính. Thường thì ngược lại: nếu TCP đi thẳng, các khớp có thể cong, đổi tốc độ khác nhau, thậm chí một khớp phải quay ngược để bù singularity. Vì vậy, controller không thể chỉ làm:

q(s) = q0 + s * (q1 - q0)

Công thức đó là MoveJ hình học. Với MoveL, ta làm trên pose:

p(s) = p0 + s * (p1 - p0)
R(s) = SLERP(R0, R1, s)
T(s) = [R(s), p(s)]
q(s) = IK(T(s), seed = q(s - ds))

Trong đó s chạy từ 0 đến 1. Phần vị trí p(s) là nội suy tuyến tính trong hệ tọa độ base hoặc world. Phần orientation không nên nội suy Euler angle từng trục, vì Euler dễ gặp wrap-around và gimbal lock. Ta dùng quaternion đơn vị và SLERP. Drake có hẳn PiecewiseQuaternionSlerp, còn Eigen cung cấp QuaternionBase::slerp(t, other). Một chi tiết quan trọng: q và -q biểu diễn cùng một orientation, nên trước khi SLERP nên chọn dấu sao cho dot product giữa hai quaternion không âm; nếu không robot có thể quay đường dài không cần thiết.

Hình học path và trajectory khác nhau

Trong bài này, ta tập trung vào path hình học trước. Path trả lời câu hỏi: TCP đi qua điểm nào? Trajectory trả lời thêm: đi qua điểm đó vào thời điểm nào, vận tốc và gia tốc bao nhiêu? Tham số moveL(pose_target, v, a) có v và a, nhưng để beginner follow, ta tách thành hai lớp:

Thực tế, lớp time parameterization có thể rất phức tạp vì v Cartesian phải chuyển sang giới hạn vận tốc khớp qua Jacobian. Bài này vẫn đưa v, a vào API để đúng hướng kiến trúc, nhưng implementation tối thiểu sẽ dùng chúng để chọn số mẫu và thời gian thô. Ở bài sau về profile và blending, ta mới làm mượt vận tốc/gia tốc nghiêm túc hơn.

Chọn bước rời rạc ds

Nếu waypoint quá thưa, đoạn TCP giữa hai nghiệm IK liên tiếp có thể bị cong khi controller cấp thấp nội suy joint. Nếu waypoint quá dày, IK chạy chậm và nhiễu số dễ tích tụ. Một rule thực dụng:

N_pos = ceil(||p1 - p0|| / max_step_m)
N_rot = ceil(angle(R0^-1 R1) / max_step_rad)
N = max(N_pos, N_rot, 2)

Với robot 6-DOF để học, max_step_m = 0.005 đến 0.02 m là hợp lý trong simulator. Với orientation, max_step_rad = 2 đến 5 độ giúp tránh quay wrist quá gấp. Nếu bạn đang điều khiển robot thật, hãy bắt đầu chậm hơn nhiều, giới hạn vùng làm việc, bật emergency stop, và không tin path trước khi FK/visualizer xác nhận.

Tại mỗi waypoint, IK nên dùng seed là nghiệm waypoint trước. Đây là bí quyết để branch IK liên tục. Nếu mỗi lần dùng cùng một seed cố định, solver có thể nhảy từ elbow-up sang elbow-down ở giữa path; TCP vẫn đúng nhưng joint path giật mạnh. IK trong bài 5 đã có DLS và joint-limit handling; MoveL chỉ bọc solver đó bằng một vòng lặp có kiểm tra liên tục.

Prototype Python: pose lerp, SLERP và verify độ thẳng

Prototype dưới đây không phụ thuộc ROS. Ta giả định bạn đã có hai hàm fk(q) và ik(T, seed) từ các bài trước. Nếu chưa có robot 6-DOF, bạn vẫn có thể mock ik hoặc thay bằng model 2-link để hiểu pipeline. Phần quan trọng là cấu trúc dữ liệu và phép đo line error.

import numpy as np
from dataclasses import dataclass
from scipy.spatial.transform import Rotation as R, Slerp

@dataclass
class Pose:
    p: np.ndarray      # shape (3,)
    q: np.ndarray      # quaternion xyzw, shape (4,)

def make_transform(pose: Pose) -> np.ndarray:
    T = np.eye(4)
    T[:3, :3] = R.from_quat(pose.q).as_matrix()
    T[:3, 3] = pose.p
    return T

def interpolate_pose(start: Pose, goal: Pose, max_step_m=0.01, max_step_deg=3.0):
    distance = np.linalg.norm(goal.p - start.p)
    r0 = R.from_quat(start.q)
    r1 = R.from_quat(goal.q)
    rot_angle = (r0.inv() * r1).magnitude()

    n_pos = int(np.ceil(distance / max_step_m)) if distance > 1e-12 else 1
    n_rot = int(np.ceil(rot_angle / np.deg2rad(max_step_deg))) if rot_angle > 1e-12 else 1
    n = max(n_pos, n_rot, 2)

    s_values = np.linspace(0.0, 1.0, n + 1)
    positions = start.p[None, :] + s_values[:, None] * (goal.p - start.p)[None, :]

    slerp = Slerp([0.0, 1.0], R.concatenate([r0, r1]))
    rotations = slerp(s_values)

    return [Pose(p=positions[i], q=rotations[i].as_quat()) for i in range(len(s_values))]

def line_error(points, p0, p1):
    points = np.asarray(points)
    d = p1 - p0
    length = np.linalg.norm(d)
    if length < 1e-12:
        return 0.0
    u = d / length
    errors = []
    for p in points:
        projection = p0 + np.dot(p - p0, u) * u
        errors.append(np.linalg.norm(p - projection))
    return float(np.max(errors))

def plan_movel(q_start, pose_goal, fk, ik, max_step_m=0.01):
    T0 = fk(q_start)
    pose_start = Pose(
        p=T0[:3, 3],
        q=R.from_matrix(T0[:3, :3]).as_quat(),
    )

    waypoints = interpolate_pose(pose_start, pose_goal, max_step_m=max_step_m)
    q_path = [np.asarray(q_start, dtype=float)]
    tcp_points = [pose_start.p]

    seed = q_path[0]
    for i, pose in enumerate(waypoints[1:], start=1):
        T_target = make_transform(pose)
        sol = ik(T_target, seed=seed)
        if not sol.success:
            raise RuntimeError(f"IK failed at waypoint {i}/{len(waypoints)-1}: {sol.reason}")
        q_path.append(sol.q)
        seed = sol.q

        T_check = fk(sol.q)
        tcp_points.append(T_check[:3, 3])

    err = line_error(tcp_points, pose_start.p, pose_goal.p)
    return np.asarray(q_path), np.asarray(tcp_points), err

Sau khi có q_path và tcp_points, hãy plot để nhìn bằng mắt. Beginner thường chỉ xem joint plots, nhưng với MoveL phải xem TCP path trong Cartesian.

import matplotlib.pyplot as plt

q_path, tcp, max_err = plan_movel(q0, pose_goal, fk=robot.fk, ik=robot.ik)

fig = plt.figure(figsize=(8, 6))
ax = fig.add_subplot(111, projection="3d")
ax.plot(tcp[:, 0], tcp[:, 1], tcp[:, 2], "o-", label="FK of IK waypoints")
ax.plot([tcp[0, 0], tcp[-1, 0]], [tcp[0, 1], tcp[-1, 1]], [tcp[0, 2], tcp[-1, 2]], "--", label="ideal line")
ax.set_title(f"MoveL TCP straightness, max error = {max_err * 1000:.2f} mm")
ax.set_xlabel("x [m]")
ax.set_ylabel("y [m]")
ax.set_zlabel("z [m]")
ax.legend()
plt.show()

Checklist pass tối thiểu:

Nếu line error lớn, nguyên nhân thường không nằm ở công thức p(s) mà ở IK không hội tụ đủ chặt, FK/IK dùng khác tool frame, hoặc robot của bạn đang execute bằng nội suy joint giữa các waypoint quá thưa.

Skeleton C++: moveL(pose_target, v, a)

Bản C++ dưới đây giả định bạn đã có class RobotModel với fk(q) và jacobian(q), cùng IKSolver từ bài 5. Ta dùng Eigen::Isometry3d cho pose và Eigen::Quaterniond cho orientation. API trả về danh sách joint waypoint; executor/time parameterizer có thể xử lý sau.

#pragma once
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <stdexcept>
#include <vector>

struct MoveLOptions {
    double tcp_speed = 0.05;       // m/s, used for timing metadata later
    double tcp_accel = 0.25;       // m/s^2
    double max_step = 0.01;        // m
    double max_rot_step = 3.0 * M_PI / 180.0;
    double line_tolerance = 0.002; // m
};

struct IKResult {
    bool success = false;
    Eigen::VectorXd q;
    double residual = 0.0;
    int iterations = 0;
    const char* reason = "";
};

struct MoveLResult {
    std::vector<Eigen::VectorXd> q_path;
    std::vector<Eigen::Isometry3d> cartesian_waypoints;
    double max_line_error = 0.0;
};

class RobotModel {
public:
    Eigen::Isometry3d fk(const Eigen::VectorXd& q) const;
    Eigen::MatrixXd jacobian(const Eigen::VectorXd& q) const;
};

class IKSolver {
public:
    IKResult solve(const Eigen::Isometry3d& target, const Eigen::VectorXd& seed) const;
};

Hàm nội suy pose:

static std::vector<Eigen::Isometry3d> interpolateCartesian(
    const Eigen::Isometry3d& start,
    const Eigen::Isometry3d& goal,
    const MoveLOptions& opt) {

    const Eigen::Vector3d p0 = start.translation();
    const Eigen::Vector3d p1 = goal.translation();
    const double dist = (p1 - p0).norm();

    Eigen::Quaterniond q0(start.rotation());
    Eigen::Quaterniond q1(goal.rotation());
    q0.normalize();
    q1.normalize();

    if (q0.dot(q1) < 0.0) {
        q1.coeffs() *= -1.0;
    }

    const double angle = q0.angularDistance(q1);
    const int n_pos = std::max(1, static_cast<int>(std::ceil(dist / opt.max_step)));
    const int n_rot = std::max(1, static_cast<int>(std::ceil(angle / opt.max_rot_step)));
    const int n = std::max({n_pos, n_rot, 2});

    std::vector<Eigen::Isometry3d> out;
    out.reserve(n + 1);

    for (int i = 0; i <= n; ++i) {
        const double s = static_cast<double>(i) / static_cast<double>(n);
        Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
        T.translation() = p0 + s * (p1 - p0);
        T.linear() = q0.slerp(s, q1).toRotationMatrix();
        out.push_back(T);
    }
    return out;
}

Hàm đo sai lệch đường thẳng bằng FK:

static double maxLineError(
    const std::vector<Eigen::Vector3d>& points,
    const Eigen::Vector3d& p0,
    const Eigen::Vector3d& p1) {

    const Eigen::Vector3d d = p1 - p0;
    const double length = d.norm();
    if (length < 1e-12) return 0.0;

    const Eigen::Vector3d u = d / length;
    double max_err = 0.0;
    for (const auto& p : points) {
        const Eigen::Vector3d proj = p0 + (p - p0).dot(u) * u;
        max_err = std::max(max_err, (p - proj).norm());
    }
    return max_err;
}

Và phần moveL chính:

MoveLResult moveL(
    const RobotModel& robot,
    const IKSolver& ik,
    const Eigen::VectorXd& q_start,
    const Eigen::Isometry3d& pose_target,
    const MoveLOptions& opt) {

    const Eigen::Isometry3d pose_start = robot.fk(q_start);
    auto waypoints = interpolateCartesian(pose_start, pose_target, opt);

    MoveLResult result;
    result.cartesian_waypoints = waypoints;
    result.q_path.reserve(waypoints.size());
    result.q_path.push_back(q_start);

    Eigen::VectorXd seed = q_start;
    for (std::size_t i = 1; i < waypoints.size(); ++i) {
        IKResult sol = ik.solve(waypoints[i], seed);
        if (!sol.success) {
            throw std::runtime_error(std::string("MoveL IK failed at waypoint ") +
                                     std::to_string(i) + ": " + sol.reason);
        }

        const double jump = (sol.q - seed).lpNorm<Eigen::Infinity>();
        if (jump > 0.35) {
            throw std::runtime_error("MoveL rejected discontinuous IK branch");
        }

        result.q_path.push_back(sol.q);
        seed = sol.q;
    }

    std::vector<Eigen::Vector3d> tcp;
    tcp.reserve(result.q_path.size());
    for (const auto& q : result.q_path) {
        tcp.push_back(robot.fk(q).translation());
    }

    result.max_line_error = maxLineError(tcp, pose_start.translation(), pose_target.translation());
    if (result.max_line_error > opt.line_tolerance) {
        throw std::runtime_error("MoveL line tolerance exceeded");
    }

    return result;
}

Ở đây v và a chưa sinh profile hoàn chỉnh, nhưng API đã đúng: tcp_speed và tcp_accel thuộc Cartesian command, không phải joint command. Khi thêm time parameterization, bạn có thể gán thời gian ban đầu T = distance / v, sau đó kiểm tra từng đoạn bằng Jacobian:

xdot = J(q) qdot
qdot = J#(q) xdot

Nếu bất kỳ joint nào vượt qdot_max, giảm v hoặc tăng thời gian đoạn. Đây là lý do bài MoveL cần tái dùng Jacobian: không chỉ để IK, mà còn để biết một tốc độ TCP có khả thi với cấu hình khớp hiện tại hay không.

CMakeLists tối thiểu

Nếu bạn tách code vào src/movel.cpp, include/movel.hpp, có thể build bằng CMake như sau:

cmake_minimum_required(VERSION 3.16)
project(classical_arm_controller LANGUAGES CXX)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(Eigen3 REQUIRED)

add_library(arm_controller
    src/robot_model.cpp
    src/ik_solver.cpp
    src/movel.cpp
)

target_include_directories(arm_controller PUBLIC include)
target_link_libraries(arm_controller PUBLIC Eigen3::Eigen)

add_executable(test_movel examples/test_movel.cpp)
target_link_libraries(test_movel PRIVATE arm_controller)

Build:

cmake -S . -B build -DCMAKE_BUILD_TYPE=Release
cmake --build build -j
./build/test_movel

test_movel.cpp nên in ít nhất số waypoint, residual IK lớn nhất, max_line_error, và pose cuối sau FK. Đừng chỉ in success=true; với MoveL, success nghĩa là path đã được verify, không chỉ IK tìm được nghiệm cuối.

Các lỗi kinh điển khi implement MoveL

Lỗi thứ nhất là nội suy orientation bằng roll-pitch-yaw. Nếu yaw đi từ 179 độ sang -179 độ, phép nội suy tuyến tính có thể quay gần 358 độ thay vì 2 độ. Quaternion SLERP xử lý vấn đề này tốt hơn, miễn là bạn chuẩn hóa quaternion và xử lý dấu.

Lỗi thứ hai là quên tool frame. MoveL nói TCP đi thẳng, không phải flange đi thẳng. Nếu robot có gripper dài 120 mm, FK phải trả pose của TCP sau khi nhân tool offset. Sai tool frame làm path nhìn có vẻ đúng ở flange nhưng đầu dụng cụ lại quét lệch.

Lỗi thứ ba là dùng IK độc lập từng waypoint. Hãy truyền seed = q_previous, kiểm tra joint jump, và ưu tiên nghiệm gần cấu hình trước. Controller công nghiệp luôn quan tâm tính liên tục của nhánh nghiệm, vì một cú wrist flip giữa path có thể nguy hiểm hơn một lỗi vị trí nhỏ.

Lỗi thứ tư là tin rằng mọi đoạn thẳng Cartesian đều khả thi. Điểm đầu và điểm cuối reachable không đảm bảo toàn bộ đoạn giữa reachable. Đường thẳng có thể đi qua singularity, joint limit, self-collision hoặc vùng cấm. Vì vậy MoveL phải fail sớm với thông báo rõ ràng: waypoint nào fail, residual bao nhiêu, joint nào gần limit.

Kết luận

MoveL là một wrapper rất rõ về mặt hình học nhưng không hề tầm thường trong controller: nội suy vị trí tuyến tính, nội suy orientation bằng SLERP, giải IK liên tục từng waypoint, rồi FK lại để kiểm chứng độ thẳng TCP. Khi làm đúng, bạn có một primitive đủ dùng cho pick-and-place chính xác, probing, bôi keo, hàn đường thẳng và mọi thao tác cần đầu dụng cụ đi theo một đoạn thẳng có kiểm soát.

Bước tiếp theo là thêm time parameterization để v và a không chỉ nằm trong API mà thật sự quyết định thời gian, vận tốc và gia tốc của từng joint. Nhưng trước khi tối ưu tốc độ, hãy giữ nguyên tắc: path hình học phải đúng trước, profile động học tính sau.

Tài liệu tham khảo

• Universal Robots Cartesian Interface: MoveL tool moves in a straight line
• MoveIt Quickstart: Use Cartesian Path để end-effector đi tuyến tính
• Eigen QuaternionBase: slerp, normalized, angularDistance
• Drake PiecewiseQuaternionSlerp: quaternions nội suy bằng spherical linear interpolation
• Fast and Accurate Relative Motion Tracking for Two Industrial Robots

Bài viết liên quan

• Inverse Kinematics: analytical vs numerical IK
• MoveJ và MoveL: từ joint space đến Cartesian space
• Jogging và frames: base, tool, world