MoveJ là lệnh đầu tiên bạn nên tự viết khi học controller robot arm cổ điển. Lý do rất đơn giản: MoveJ không yêu cầu giải IK liên tục, không cần giữ TCP đi theo đường thẳng, không cần xử lý orientation interpolation phức tạp. Controller chỉ cần lấy cấu hình khớp hiện tại q_start, cấu hình khớp đích q_target, rồi sinh một chuỗi setpoint theo thời gian sao cho mọi khớp bắt đầu cùng lúc, kết thúc cùng lúc, và không vượt giới hạn vận tốc/gia tốc.
Trong các tài liệu công nghiệp, Universal Robots mô tả movej(q, a, v, t, r) là chuyển động "linear in joint-space". Tài liệu PolyScope cũng nói rõ MoveJ được tính trong joint space, các khớp được điều khiển để hoàn thành chuyển động cùng lúc, và vì vậy đường TCP thường là đường cong chứ không phải đường thẳng. Đây chính là ý tưởng chúng ta sẽ tự triển khai trong bài này.
Nếu bạn đã đọc bài về frame và jogging, bạn đã thấy rằng lệnh online như jogging cần servo loop phản ứng theo từng chu kỳ. MoveJ khác hơn: ta biết trước điểm đầu và điểm cuối, nên có thể sinh trajectory offline trước khi gửi xuống loop. Nếu bạn đã đọc bài C++/CMake setup, cấu trúc project ở cuối bài này sẽ rất quen thuộc.
Trong stack sản phẩm, MoveJ thường nằm bên dưới tầng task logic. Một policy vision-language như trong bài VLA models cho robotics có thể chọn pose hoặc joint goal, nhưng MoveJ vẫn là lớp biến goal đó thành setpoint an toàn. Khi nối sang robot thật qua ROS 2, bài ROS 2 Control Hardware Interface sẽ giúp bạn hiểu phần driver nhận trajectory sau khi generator đã sinh xong.
Nguồn ảnh: Universal Robots manual. Hình này minh họa ý tưởng tăng tốc, chạy đều, rồi giảm tốc. Trong bài này ta dùng quintic time scaling để dễ code và mượt ở đầu/cuối; bài 11 sẽ đi sâu vào trapezoid, S-curve và jerk-limited profile.
MoveJ thực sự làm gì?
Giả sử robot arm có 6 khớp:
q_start = [q1, q2, q3, q4, q5, q6]
q_target = [g1, g2, g3, g4, g5, g6]
MoveJ không nói: "TCP hãy đi theo đường thẳng từ pose A tới pose B". MoveJ nói: "Mỗi joint hãy đi từ góc hiện tại tới góc đích theo một profile thời gian chung". Nói cách khác, ta nội suy trong không gian khớp:
q(t) = q_start + s(t) * (q_target - q_start)
Trong đó s(t) là một số chạy từ 0 tới 1. Khi s = 0, robot ở q_start. Khi s = 1, robot ở q_target. Nếu mọi khớp dùng cùng một s(t), tất cả khớp sẽ hoàn thành đúng cùng thời điểm.
Điểm quan trọng là s(t) không nên là đường thẳng s = t / T. Nếu nội suy tuyến tính trực tiếp, vận tốc nhảy từ 0 lên giá trị hằng ngay tại thời điểm bắt đầu, rồi nhảy về 0 ở cuối. Robot thật không thích điều đó. Drive sẽ bị giật, tracking error tăng, và nếu controller bên dưới cứng quá có thể gây rung.
Ta cần một time law mượt hơn. Một lựa chọn rất phổ biến cho bài học nhập môn là quintic polynomial:
u = t / T, 0 <= u <= 1
s(u) = 10u^3 - 15u^4 + 6u^5
sd(u) = 30u^2 - 60u^3 + 30u^4
sdd(u)= 60u - 180u^2 + 120u^3
Với công thức này, s(0)=0, s(1)=1, vận tốc đầu/cuối bằng 0, gia tốc đầu/cuối cũng bằng 0. Robotics Toolbox for Python gọi jtraj(q0, qf, t) là joint-space trajectory và dùng quintic bậc 5 với điều kiện biên vận tốc/gia tốc mặc định bằng 0. Vì vậy prototype Python của chúng ta sẽ rất gần phong cách jtraj.
"Đường ngắn nhất" trong joint space
Với khớp revolute, cùng một tư thế vật lý có thể viết bằng nhiều góc khác nhau, ví dụ 179 deg và -181 deg. Khi người dùng đưa q_target, controller phải quyết định đi hướng nào. Với MoveJ cơ bản, ta thường chọn hiệu góc ngắn nhất:
delta = wrap_to_pi(q_target - q_start)
wrap_to_pi đưa hiệu góc về khoảng [-pi, pi]. Ví dụ từ 170 deg tới -170 deg, đi trực tiếp theo phép trừ thường là -340 deg, nhưng đường ngắn nhất là +20 deg. Với robot công nghiệp thật, điều này còn phụ thuộc joint limit, cấu hình cable, hard stop và khả năng multi-turn của từng trục. Bài này dùng quy tắc đường ngắn nhất để người mới dễ hiểu; khi tích hợp robot thật, bạn nên để mỗi joint khai báo continuous hay bounded.
Quy tắc an toàn tối thiểu:
if joint is continuous:
delta = shortest_angular_distance(q_start, q_target)
else:
delta = q_target - q_start
check q_target inside [q_min, q_max]
Đồng bộ tất cả khớp tới đích cùng lúc
Nếu joint 1 cần đi 10 độ, joint 2 cần đi 90 độ, joint 3 cần đi 5 độ, ta không muốn joint 3 chạy xong quá sớm rồi đứng đợi. MoveJ công nghiệp thường đồng bộ các trục bằng một thời gian chuyển động chung. Khớp cần nhiều thời gian nhất là khớp "leading axis" hoặc "bottleneck". Các khớp còn lại đi chậm hơn tương ứng.
Với time law quintic, ta có thể tính gần đúng thời gian tối thiểu theo giới hạn vận tốc và gia tốc. Vì:
q_i(t) = q0_i + delta_i * s(t)
dq_i(t) = delta_i * sd(u) / T
ddq_i(t)= delta_i * sdd(u) / T^2
Giá trị lớn nhất của sd(u) là 1.875. Giá trị lớn nhất tuyệt đối của sdd(u) xấp xỉ 5.7735. Vì vậy để không vượt giới hạn:
T >= abs(delta_i) * 1.875 / v_limit_i
T >= sqrt(abs(delta_i) * 5.7735 / a_limit_i)
Ta tính hai giá trị này cho từng khớp, lấy max trên tất cả khớp. Đó là thời gian chung T. Khi user truyền v và a như URScript movej(q, a=1.4, v=1.05), cách đơn giản nhất là áp dụng cùng limit cho mọi joint. Trong controller nghiêm túc hơn, mỗi joint nên có limit riêng đọc từ robot model hoặc file cấu hình như joint_limits.yaml. MoveIt cũng xử lý time parameterization dựa trên giới hạn vận tốc/gia tốc của từng joint trong URDF hoặc joint_limits.yaml.
Nguồn ảnh: MoveIt documentation. MoveJ tự viết trong bài này là bản nhỏ của tầng trajectory generation/time parameterization: ta đã có đường joint-space và cần gán thời gian hợp lệ.
Prototype Python: jtraj-style quintic
Tạo file movej_quintic.py trong một thư mục thử nghiệm bất kỳ:
import numpy as np
import matplotlib.pyplot as plt
def wrap_to_pi(x):
return (x + np.pi) % (2.0 * np.pi) - np.pi
def quintic_time_scaling(t, T):
u = np.clip(t / T, 0.0, 1.0)
s = 10*u**3 - 15*u**4 + 6*u**5
sd = (30*u**2 - 60*u**3 + 30*u**4) / T
sdd = (60*u - 180*u**2 + 120*u**3) / (T*T)
return s, sd, sdd
def compute_duration(delta, v_limit, a_limit):
delta = np.abs(delta)
t_vel = np.max(delta * 1.875 / v_limit)
t_acc = np.max(np.sqrt(delta * 5.7735 / a_limit))
return max(float(t_vel), float(t_acc), 0.1)
def movej(q_start, q_target, v_limit=1.2, a_limit=2.5, dt=0.01, continuous=True):
q_start = np.asarray(q_start, dtype=float)
q_target = np.asarray(q_target, dtype=float)
if continuous:
delta = wrap_to_pi(q_target - q_start)
else:
delta = q_target - q_start
v = np.full_like(q_start, v_limit, dtype=float)
a = np.full_like(q_start, a_limit, dtype=float)
T = compute_duration(delta, v, a)
time = np.arange(0.0, T + dt, dt)
q = []
qd = []
qdd = []
for t in time:
s, sd, sdd = quintic_time_scaling(t, T)
q.append(q_start + delta * s)
qd.append(delta * sd)
qdd.append(delta * sdd)
return time, np.array(q), np.array(qd), np.array(qdd), T
if __name__ == "__main__":
q0 = np.deg2rad([0, -60, 80, 0, 45, 10])
q1 = np.deg2rad([90, -20, 30, -80, 10, 170])
t, q, qd, qdd, T = movej(q0, q1, v_limit=1.0, a_limit=2.0, dt=0.01)
print(f"Move duration: {T:.3f} s")
print(f"Max speed per joint deg/s: {np.rad2deg(np.max(np.abs(qd), axis=0))}")
print(f"Max accel per joint deg/s^2: {np.rad2deg(np.max(np.abs(qdd), axis=0))}")
names = [f"J{i+1}" for i in range(q.shape[1])]
fig, axes = plt.subplots(2, 1, figsize=(11, 7), sharex=True)
for i, name in enumerate(names):
axes[0].plot(t, np.rad2deg(q[:, i]), label=name)
axes[1].plot(t, np.rad2deg(qd[:, i]), label=name)
axes[0].set_ylabel("Position (deg)")
axes[0].set_title("MoveJ joint positions - quintic time scaling")
axes[0].grid(True)
axes[0].legend(ncol=3)
axes[1].set_ylabel("Velocity (deg/s)")
axes[1].set_xlabel("Time (s)")
axes[1].set_title("MoveJ joint velocities")
axes[1].grid(True)
axes[1].legend(ncol=3)
plt.tight_layout()
plt.show()
Chạy:
python3 movej_quintic.py
Bạn sẽ thấy mọi đường position bắt đầu và kết thúc phẳng, còn đường velocity có dạng "chuông" mượt. Khớp có delta lớn nhất sẽ có vận tốc lớn nhất. Các khớp khác dùng cùng thời gian T, nên chúng tự động đi chậm hơn và về đích đúng lúc.
Điểm cần chú ý: prototype này chưa check va chạm, chưa check self-collision, chưa biết robot có chạm fixture hay không. MoveJ chỉ đảm bảo quỹ đạo trong joint space mượt và không vượt limit động học mà ta đưa vào. Nếu bạn cần tránh vật cản, hãy đọc tiếp bài motion planning trong series, còn nếu cần hiểu các loại đường đi trước khi planning, xem bài về trajectory types.
Nguồn ảnh: Robotics Toolbox for Python documentation. Toolbox này tiện để kiểm chứng nhanh jtraj, FK và hình dạng chuyển động trước khi bạn viết bản C++ tối giản.
Thiết kế API C++ cho MoveJ
Ta muốn một API nhỏ, không phụ thuộc ROS, dễ nhúng vào controller:
Trajectory moveJ(q_current, q_target, v_limit, a_limit, dt);
Đầu ra nên là vector các setpoint:
time, q[0..N-1], qd[0..N-1], qdd[0..N-1]
Trong servo loop thật, mỗi chu kỳ bạn lấy setpoint kế tiếp và gửi xuống joint driver. Trong simulation đơn giản, ta ghi CSV rồi plot lại.
Tạo cấu trúc thư mục:
movej_demo/
CMakeLists.txt
src/
main.cpp
Nội dung CMakeLists.txt:
cmake_minimum_required(VERSION 3.16)
project(movej_demo LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
add_executable(movej_demo src/main.cpp)
target_compile_options(movej_demo PRIVATE -Wall -Wextra -Wpedantic)
Nội dung src/main.cpp:
#include <algorithm>
#include <array>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <vector>
constexpr std::size_t DOF = 6;
constexpr double kPi = 3.14159265358979323846;
using Vec = std::array<double, DOF>;
struct Setpoint {
double t{};
Vec q{};
Vec qd{};
Vec qdd{};
};
struct Trajectory {
double duration{};
std::vector<Setpoint> points;
};
double wrapToPi(double x) {
while (x > kPi) x -= 2.0 * kPi;
while (x < -kPi) x += 2.0 * kPi;
return x;
}
Vec subtractTarget(const Vec& q0, const Vec& q1, bool continuous) {
Vec delta{};
for (std::size_t i = 0; i < DOF; ++i) {
const double raw = q1[i] - q0[i];
delta[i] = continuous ? wrapToPi(raw) : raw;
}
return delta;
}
double computeDuration(const Vec& delta, const Vec& v_limit, const Vec& a_limit) {
double T = 0.1;
for (std::size_t i = 0; i < DOF; ++i) {
if (v_limit[i] <= 0.0 || a_limit[i] <= 0.0) {
throw std::runtime_error("Velocity and acceleration limits must be positive");
}
const double d = std::abs(delta[i]);
const double t_vel = d * 1.875 / v_limit[i];
const double t_acc = std::sqrt(d * 5.7735 / a_limit[i]);
T = std::max(T, std::max(t_vel, t_acc));
}
return T;
}
void quintic(double t, double T, double& s, double& sd, double& sdd) {
const double u = std::clamp(t / T, 0.0, 1.0);
const double u2 = u * u;
const double u3 = u2 * u;
const double u4 = u3 * u;
const double u5 = u4 * u;
s = 10.0 * u3 - 15.0 * u4 + 6.0 * u5;
sd = (30.0 * u2 - 60.0 * u3 + 30.0 * u4) / T;
sdd = (60.0 * u - 180.0 * u2 + 120.0 * u3) / (T * T);
}
Trajectory moveJ(const Vec& q0, const Vec& q_target, const Vec& v_limit,
const Vec& a_limit, double dt, bool continuous = true) {
if (dt <= 0.0) {
throw std::runtime_error("dt must be positive");
}
const Vec delta = subtractTarget(q0, q_target, continuous);
const double T = computeDuration(delta, v_limit, a_limit);
Trajectory traj;
traj.duration = T;
for (double t = 0.0; t < T + 0.5 * dt; t += dt) {
const double tc = std::min(t, T);
double s = 0.0, sd = 0.0, sdd = 0.0;
quintic(tc, T, s, sd, sdd);
Setpoint p;
p.t = tc;
for (std::size_t i = 0; i < DOF; ++i) {
p.q[i] = q0[i] + delta[i] * s;
p.qd[i] = delta[i] * sd;
p.qdd[i] = delta[i] * sdd;
}
traj.points.push_back(p);
}
return traj;
}
void writeCsv(const Trajectory& traj, const std::string& path) {
std::ofstream out(path);
out << "t";
for (std::size_t i = 0; i < DOF; ++i) out << ",q" << i + 1;
for (std::size_t i = 0; i < DOF; ++i) out << ",qd" << i + 1;
for (std::size_t i = 0; i < DOF; ++i) out << ",qdd" << i + 1;
out << "\n";
out << std::fixed << std::setprecision(8);
for (const auto& p : traj.points) {
out << p.t;
for (double x : p.q) out << "," << x;
for (double x : p.qd) out << "," << x;
for (double x : p.qdd) out << "," << x;
out << "\n";
}
}
int main() {
const Vec q0 = {0.0, -1.0472, 1.3963, 0.0, 0.7854, 0.1745};
const Vec q1 = {1.5708, -0.3491, 0.5236, -1.3963, 0.1745, 2.9671};
const Vec v = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
const Vec a = {2.0, 2.0, 2.0, 2.0, 2.0, 2.0};
const double dt = 0.008; // 125 Hz command stream
const auto traj = moveJ(q0, q1, v, a, dt, true);
writeCsv(traj, "movej.csv");
std::cout << "Generated " << traj.points.size() << " setpoints\n";
std::cout << "Duration: " << traj.duration << " s\n";
std::cout << "CSV: movej.csv\n";
return 0;
}
Build:
mkdir -p build
cd build
cmake ..
cmake --build .
./movej_demo
Sau khi chạy, bạn có movej.csv. Đây là "playback log" đơn giản: mỗi dòng là một setpoint theo thời gian. Trong controller thật, thay vì ghi CSV, bạn sẽ gửi p.q vào interface position command, hoặc gửi cả p.q, p.qd, p.qdd nếu driver hỗ trợ feedforward.
Playback trong sim tối giản
Tạo playback.py cạnh file movej.csv:
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
data = np.genfromtxt("movej.csv", delimiter=",", names=True)
t = data["t"]
q = np.vstack([data[f"q{i}"] for i in range(1, 7)]).T
fig, ax = plt.subplots(figsize=(8, 4))
bars = ax.bar([f"J{i}" for i in range(1, 7)], np.rad2deg(q[0]))
ax.set_ylim(-190, 190)
ax.set_ylabel("Joint position (deg)")
ax.set_title("MoveJ playback from C++ CSV")
time_text = ax.text(0.02, 0.92, "", transform=ax.transAxes)
def update(frame):
for bar, value in zip(bars, np.rad2deg(q[frame])):
bar.set_height(value)
time_text.set_text(f"t = {t[frame]:.3f} s")
return list(bars) + [time_text]
ani = FuncAnimation(fig, update, frames=len(t), interval=8, blit=True)
plt.show()
Chạy:
python3 playback.py
Đây chưa phải simulator hình học 3D, nhưng nó kiểm tra được điều quan trọng nhất của MoveJ: setpoint có đều theo thời gian không, các khớp có về đích cùng lúc không, và vận tốc có mượt không. Nếu muốn gắn vào simulator thật, bạn chỉ cần thay phần bar chart bằng FK + viewer. Có thể dùng Robotics Toolbox, PyBullet, MuJoCo, hoặc viewer nội bộ của dự án.
Checklist an toàn cho MoveJ đầu tiên
Trước khi gửi MoveJ xuống robot thật, hãy kiểm tra các điểm sau:
| Kiểm tra | Vì sao quan trọng |
|---|---|
q_target nằm trong joint limit |
Tránh gửi lệnh không thể thực thi |
delta dùng đúng wrap hay không |
Tránh trục quay vòng dài ngoài ý muốn |
max(abs(qd)) <= v_limit |
Bảo vệ drive và gearbox |
max(abs(qdd)) <= a_limit |
Giảm rung và tracking error |
| Setpoint period đúng với servo loop | Tránh jitter do sample rate lệch |
| Start từ trạng thái thật của robot | Tránh nhảy setpoint ở mẫu đầu tiên |
| Có stop condition | Robot phải dừng khi lỗi tracking, E-stop, protective stop |
Một lỗi beginner hay gặp là lấy q_start từ biến nội bộ cũ thay vì encoder hiện tại. Nếu robot bị lệch vì người vận hành kéo tay, teach pendant jog, hoặc restart controller, setpoint đầu tiên sẽ nhảy. Hãy luôn seed MoveJ bằng trạng thái feedback mới nhất.
MoveJ khác MoveL ở đâu?
Bài này tập trung vào MoveJ, nhưng cần nhắc ngắn gọn để bạn không dùng nhầm:
| Lệnh | Nội suy | Ưu điểm | Nhược điểm |
|---|---|---|---|
| MoveJ | Joint space | Nhanh, ổn định, dễ giới hạn vận tốc/gia tốc khớp | TCP đi cong, khó dự đoán |
| MoveL | Cartesian space | TCP đi thẳng, hợp cho approach/insert/weld | Cần IK liên tục, dễ gặp singularity |
MoveJ phù hợp cho "free move": đi từ home tới gần vùng thao tác, đi giữa hai vùng không có vật cản gần tool, hoặc đưa robot về pose an toàn. MoveL phù hợp khi TCP phải đi theo đường thẳng, ví dụ đưa gripper xuống theo phương Z để gắp vật. Trong pick-and-place thực tế, công thức phổ biến là MoveJ tới điểm approach, MoveL đi xuống, MoveL rút lên, rồi MoveJ sang vùng tiếp theo.
Liên hệ tới profile vận tốc ở bài 11
Quintic time scaling là lựa chọn tốt để học vì công thức ngắn và mượt ở biên. Nhưng nó chưa phải profile tối ưu thời gian. Controller công nghiệp thường dùng trapezoid, S-curve hoặc jerk-limited trajectory. Ruckig, chẳng hạn, sinh trajectory từ trạng thái hiện tại tới trạng thái đích với ràng buộc vận tốc, gia tốc và jerk, đồng thời có thể chạy online trong control loop. MoveIt 2 cũng có bước time parameterization và có thể dùng Ruckig smoothing để giảm jerk sau khi planner sinh đường đi.
Trong bài 11 về velocity profiles, ta sẽ thay phần quintic() bằng trapezoid, S-curve và jerk-limited profile. Nhưng kiến trúc tổng thể vẫn giữ nguyên:
q_start, q_target
-> delta theo từng joint
-> chọn thời gian/profile chung
-> sample q, qd, qdd theo dt
-> gửi setpoint xuống servo loop
Khi controller cần giới hạn jerk và phản ứng online, ta thay time law tự viết bằng thư viện trajectory generation chuyên dụng. Ảnh featured của bài dùng plot trajectory từ Ruckig để nhấn mạnh cầu nối này giữa MoveJ cơ bản và profile vận tốc nâng cao.
Tài liệu tham khảo
- Universal Robots Script Manual: movej(q, a, v, t, r)
- Universal Robots PolyScope Move command documentation
- Robotics Toolbox for Python: trajectories and jtraj
- MoveIt 2 documentation: Time Parameterization
- Ruckig documentation: online trajectory generation tutorial
Kết luận
Bạn đã có một MoveJ tối giản nhưng đúng cấu trúc: chọn đường ngắn nhất trong joint space, tính thời gian chung từ giới hạn vận tốc/gia tốc, dùng quintic time scaling để sinh q, qd, qdd, build bằng CMake, xuất CSV và playback trong sim nhỏ. Đây là nền để thêm joint limit per-axis, collision check, blending, profile jerk-limited và interface robot thật.
Đừng đánh giá thấp bài này. Một MoveJ sạch là viên gạch đầu tiên của controller robot arm. Khi MoveJ đã chạy đúng, bạn có thể tự tin chuyển sang MoveL, MoveC, blending và motion planning mà không bị mơ hồ về phần setpoint theo thời gian.
