Đây là bài cuối cùng trong series 15 bài về Robot Arm Controller cổ điển. Chúng ta đã đi qua FK/IK và Jacobian, trajectory planning, MoveJ/MoveL/MoveC, blending, motion profiles, jogging và servo control, và motion planning tránh va chạm với MoveIt2. Bây giờ đến bài test thực tế nhất: đưa tất cả lên robot thật.
Mục tiêu bài này: sau khi đọc xong, bạn có thể kết nối ROS 2 với UR robot (hoặc URSim), gửi trajectory qua joint_trajectory_controller, dùng ur_rtde bằng cả Python lẫn C++, và có đủ logging để debug khi có vấn đề.
Triết lý simulation-first
Nguyên tắc vàng khi làm với robot thật: không bao giờ chạy code mới trực tiếp lên hardware. Robot arm nặng vài chục kg, tốc độ di chuyển lên đến 3 m/s — một lệnh sai có thể phá fixture, gây chấn thương, hoặc làm hỏng robot.
Workflow chuẩn của mọi robotics engineer:
Code mới → URSim (virtual robot) → kiểm tra kỹ → UR robot thật
URSim là bộ mô phỏng chính thức của Universal Robots, chạy gần như y hệt robot thật qua giao thức mạng. Nếu code chạy đúng trên URSim → an toàn hơn rất nhiều khi chuyển sang hardware thật.
Bước 1 — Chạy URSim với Docker
Universal Robots cung cấp Docker image chính thức, không cần cài VirtualBox:
# Chạy URSim UR5e (e-Series)
docker run --rm -it \
-p 5900:5900 \
-p 6080:6080 \
-p 29999:29999 \
-p 30001-30004:30001-30004 \
-e ROBOT_MODEL=UR5 \
universalrobots/ursim_e-series
Mở browser: http://localhost:6080/vnc.html → Polyscope UI xuất hiện, robot ở trạng thái "Normal".
Các ports quan trọng cần biết:
| Port | Giao thức | Dùng cho |
|---|---|---|
| 30001 | Primary Interface | Robot state, program upload |
| 30002 | Secondary Interface | Program execution |
| 30004 | RTDE | Real-time data exchange (ur_rtde) |
| 29999 | Dashboard Server | Power on/off, brake release |
Nếu dùng mạng host thay vì Docker bridge, IP của URSim thường là 192.168.56.101.
Bước 2 — Kiến trúc ros2_control
ros2_control là hardware abstraction layer chuẩn của ROS 2. Thay vì viết driver riêng cho từng robot, tất cả đều nói chuyện qua cùng một interface. Với UR robot, UR cung cấp ur_robot_driver implement interface này:
ros2_control stack:
┌──────────────────────────────────────────┐
│ Controller Manager │ ← quản lý lifecycle controllers
├─────────────────────┬────────────────────┤
│ scaled_JTC │ io_status_ctrl │ ← Controllers (plugin)
├─────────────────────┴────────────────────┤
│ Resource Manager │ ← quản lý hardware resources
├──────────────────────────────────────────┤
│ UrPositionHardwareInterface │ ← ur_robot_driver hardware plugin
├──────────────────────────────────────────┤
│ UR Robot / URSim │ ← hardware thật / giả lập
└──────────────────────────────────────────┘
scaled_joint_trajectory_controller là controller mặc định của UR driver — khác với joint_trajectory_controller chuẩn ở điểm quan trọng: nó đọc speed scaling từ robot (slider trên Teach Pendant) và safety compliance. Khi robot bị safeguard stop, trajectory tự dừng đúng chỗ và tiếp tục sau khi resume — không bị jump.
Bước 3 — Cài đặt và launch UR ROS2 Driver
# ROS 2 Humble + Ubuntu 22.04 (khuyến nghị)
sudo apt install ros-humble-ur
# Hoặc build từ source (nếu cần features mới nhất):
mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
git clone https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git
cd ~/ros2_ws && colcon build --packages-select ur_robot_driver
source install/setup.bash
Launch driver kết nối URSim
# Terminal 1: Launch driver (URSim IP mặc định 192.168.56.101)
ros2 launch ur_robot_driver ur_control.launch.py \
ur_type:=ur5e \
robot_ip:=192.168.56.101 \
launch_rviz:=true
# Hoặc fake hardware (không cần URSim / robot):
ros2 launch ur_robot_driver ur_control.launch.py \
ur_type:=ur5e \
robot_ip:=yyy.yyy.yyy.yyy \
use_fake_hardware:=true \
launch_rviz:=true
Lưu ý quan trọng: Để driver kết nối được với URSim/robot thật, bạn phải:
- Cài URCap "External Control" trên Polyscope
- Cấu hình IP của máy tính chạy ROS 2 trong URCap
- Tạo và chạy program "External Control" trên Teach Pendant
Sau khi launch thành công, verify controllers:
# Terminal 2: Kiểm tra controllers
ros2 control list_controllers
Output mong đợi:
scaled_joint_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] active
io_and_status_controller[ur_controllers/GPIOController] active
joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
Bước 4 — Gửi JointTrajectory qua ROS 2
trajectory_msgs/JointTrajectory là message type chuẩn để gửi quỹ đạo. Mỗi waypoint là JointTrajectoryPoint chứa positions, velocities, và time_from_start.
Python — Gửi trajectory qua FollowJointTrajectory action
#!/usr/bin/env python3
"""Gửi trajectory đến UR robot qua ROS 2 action."""
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from builtin_interfaces.msg import Duration
import math
class TrajectoryExecutor(Node):
def __init__(self):
super().__init__('trajectory_executor')
self._action_client = ActionClient(
self,
FollowJointTrajectory,
'/scaled_joint_trajectory_controller/follow_joint_trajectory'
)
def send_trajectory(self):
self._action_client.wait_for_server()
self.get_logger().info('Action server ready.')
goal = FollowJointTrajectory.Goal()
goal.trajectory.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint',
'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]
# Waypoint 1: home position (thường an toàn để bắt đầu)
p1 = JointTrajectoryPoint()
p1.positions = [0.0, -math.pi/2, 0.0, -math.pi/2, 0.0, 0.0]
p1.velocities = [0.0] * 6
p1.time_from_start = Duration(sec=3, nanosec=0)
# Waypoint 2: pick position (điều chỉnh theo môi trường thực tế)
p2 = JointTrajectoryPoint()
p2.positions = [math.pi/4, -math.pi/3, math.pi/2, -math.pi/4, math.pi/2, 0.0]
p2.velocities = [0.0] * 6
p2.time_from_start = Duration(sec=6, nanosec=0)
goal.trajectory.points = [p1, p2]
self.get_logger().info('Sending trajectory goal...')
future = self._action_client.send_goal_async(goal)
rclpy.spin_until_future_complete(self, future)
goal_handle = future.result()
if not goal_handle.accepted:
self.get_logger().error('Goal rejected!')
return
result_future = goal_handle.get_result_async()
rclpy.spin_until_future_complete(self, result_future)
result = result_future.result().result
self.get_logger().info(f'Done. error_code={result.error_code}')
def main():
rclpy.init()
node = TrajectoryExecutor()
node.send_trajectory()
rclpy.shutdown()
if __name__ == '__main__':
main()
Bước 5 — ur_rtde Python: prototype nhanh trước khi viết C++
ur_rtde là thư viện low-level kết nối trực tiếp qua RTDE protocol (port 30004), không cần ROS 2. Dùng Python bindings để validate logic trước khi implement C++.
pip install ur-rtde
moveJ, moveL và log tracking error
import rtde_control
import rtde_receive
import math
ROBOT_IP = "192.168.56.101" # URSim — đổi thành IP robot thật khi deploy
rtde_c = rtde_control.RTDEControlInterface(ROBOT_IP)
rtde_r = rtde_receive.RTDEReceiveInterface(ROBOT_IP)
# Đọc state hiện tại
q_actual = rtde_r.getActualQ()
print(f"Hiện tại: {[f'{v:.4f}' for v in q_actual]}")
# MoveJ — di chuyển theo joint space (an toàn, predictable)
q_target = [0.0, -math.pi/2, math.pi/2, -math.pi/2, -math.pi/2, 0.0]
rtde_c.moveJ(q_target, speed=1.05, acceleration=1.4) # blocking
# Log tracking error ngay sau khi dừng
q_after = rtde_r.getActualQ()
tracking_err = [abs(q_target[i] - q_after[i]) for i in range(6)]
print(f"Tracking error (rad): {[f'{e:.6f}' for e in tracking_err]}")
print(f"Max error: {max(tracking_err):.6f} rad")
# MoveL — di chuyển TCP theo đường thẳng trong Cartesian space
# pose = [x, y, z, rx, ry, rz] — meters + axis-angle (radians)
pose_target = [0.3, -0.4, 0.5, 0.0, math.pi, 0.0]
rtde_c.moveL(pose_target, speed=0.25, acceleration=1.2)
tcp_after = rtde_r.getActualTCPPose()
print(f"TCP sau moveL: {[f'{v:.4f}' for v in tcp_after]}")
rtde_c.disconnect()
rtde_r.disconnect()
servoJ — Real-time streaming ở 500 Hz
import rtde_control
import rtde_receive
import time
import math
rtde_c = rtde_control.RTDEControlInterface(ROBOT_IP)
rtde_r = rtde_receive.RTDEReceiveInterface(ROBOT_IP)
DT = 0.002 # 500 Hz control loop (khuyến nghị tối thiểu 125 Hz)
LOOKAHEAD = 0.1 # Lookahead time (0.03–0.2s) — tăng = mượt hơn nhưng lag hơn
GAIN = 300 # Servo gain (100–2000) — tăng = bám sát hơn nhưng rung hơn
q_start = rtde_r.getActualQ()
t = 0.0
duration = 3.0
while t < duration:
loop_start = time.time()
# Trajectory: joint 0 dao động ±0.2 rad (sin wave)
q_cmd = list(q_start)
q_cmd[0] = q_start[0] + 0.2 * math.sin(2 * math.pi * t / duration)
rtde_c.servoJ(q_cmd, velocity=0.0, acceleration=0.0,
dt=DT, lookahead_time=LOOKAHEAD, gain=GAIN)
q_actual = rtde_r.getActualQ()
err = abs(q_cmd[0] - q_actual[0])
loop_ms = (time.time() - loop_start) * 1000
print(f"t={t:.3f}s | cmd={q_cmd[0]:.4f} | act={q_actual[0]:.4f} "
f"| err={err:.4f} rad | loop={loop_ms:.1f}ms")
elapsed = time.time() - loop_start
if elapsed < DT:
time.sleep(DT - elapsed)
t += DT
rtde_c.servoStop()
rtde_c.disconnect()
rtde_r.disconnect()
Bước 6 — ur_rtde C++: CMakeLists.txt và implementation
Cài ur_rtde từ source
git clone https://gitlab.com/sdurobotics/ur_rtde.git
cd ur_rtde && mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
sudo make install # cài vào /usr/local/
CMakeLists.txt
cmake_minimum_required(VERSION 3.16)
project(ur_rtde_controller CXX)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# Tìm ur_rtde (sau khi sudo make install)
find_package(ur_rtde REQUIRED)
# Nếu cài vào path không chuẩn:
# find_package(ur_rtde REQUIRED PATHS "/usr/local/lib/cmake/ur_rtde")
add_executable(ur_controller src/ur_controller.cpp)
target_link_libraries(ur_controller PRIVATE ur_rtde::rtde)
Build và chạy:
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
# Kết nối URSim
./ur_controller 192.168.56.101
# Khi sẵn sàng robot thật (thay IP)
./ur_controller 192.168.1.100
src/ur_controller.cpp — Implementation đầy đủ với logging
#include <ur_rtde/rtde_control_interface.h>
#include <ur_rtde/rtde_receive_interface.h>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <chrono>
#include <cmath>
#include <numeric>
#include <algorithm>
#include <thread>
#include <string>
using namespace ur_rtde;
using Clock = std::chrono::high_resolution_clock;
// ── CSV Logger ───────────────────────────────────────────────────────────────
class RobotLogger {
std::ofstream file_;
public:
explicit RobotLogger(const std::string& filename) : file_(filename) {
file_ << "ts_ms,"
<< "j0t,j1t,j2t,j3t,j4t,j5t,"
<< "j0a,j1a,j2a,j3a,j4a,j5a,"
<< "max_err_rad,latency_ms\n";
}
void log(double ts,
const std::vector<double>& target,
const std::vector<double>& actual,
double max_err, double latency) {
file_ << std::fixed << std::setprecision(6) << ts;
for (double v : target) file_ << ',' << v;
for (double v : actual) file_ << ',' << v;
file_ << ',' << max_err << ',' << latency << '\n';
}
};
// ── Helper: tính tracking error ──────────────────────────────────────────────
static double maxError(const std::vector<double>& a,
const std::vector<double>& b) {
double mx = 0.0;
for (std::size_t i = 0; i < a.size(); ++i)
mx = std::max(mx, std::abs(a[i] - b[i]));
return mx;
}
// ── MoveJ blocking với log ───────────────────────────────────────────────────
void executeMoveJ(RTDEControlInterface& rtde_c,
RTDEReceiveInterface& rtde_r,
const std::vector<double>& q_target,
double speed = 1.05, double accel = 1.4) {
std::cout << "MoveJ → [";
for (double q : q_target) std::cout << std::setprecision(3) << q << ' ';
std::cout << "]\n";
auto t0 = Clock::now();
rtde_c.moveJ(q_target, speed, accel); // blocking — chờ đến khi dừng
double elapsed = std::chrono::duration<double, std::milli>(
Clock::now() - t0).count();
std::vector<double> q_actual = rtde_r.getActualQ();
double err = maxError(q_target, q_actual);
std::cout << std::fixed << std::setprecision(4)
<< " Done: t=" << elapsed << "ms, max_err=" << err << " rad\n";
}
// ── MoveL blocking với log ───────────────────────────────────────────────────
void executeMoveL(RTDEControlInterface& rtde_c,
RTDEReceiveInterface& rtde_r,
const std::vector<double>& pose_target,
double speed = 0.25, double accel = 1.2) {
std::cout << "MoveL → [";
for (double p : pose_target) std::cout << std::setprecision(4) << p << ' ';
std::cout << "]\n";
auto t0 = Clock::now();
rtde_c.moveL(pose_target, speed, accel);
double elapsed = std::chrono::duration<double, std::milli>(
Clock::now() - t0).count();
std::vector<double> tcp = rtde_r.getActualTCPPose();
double err = maxError(pose_target, tcp);
std::cout << std::fixed << std::setprecision(4)
<< " Done: t=" << elapsed << "ms, max_err=" << err << " m/rad\n";
}
// ── ServoJ real-time loop ─────────────────────────────────────────────────────
void executeServoJ(RTDEControlInterface& rtde_c,
RTDEReceiveInterface& rtde_r,
double duration_s = 3.0,
const std::string& log_file = "servo_log.csv") {
const double DT = 0.002; // 500 Hz
const double LOOKAHEAD = 0.1;
const int GAIN = 300;
RobotLogger logger(log_file);
std::vector<double> q_start = rtde_r.getActualQ();
std::cout << "ServoJ demo start (" << duration_s << "s)...\n";
double t = 0.0;
int samples = 0;
double sum_err = 0.0, max_lat = 0.0;
auto t_begin = Clock::now();
while (t < duration_s) {
auto loop_start = Clock::now();
// Sin wave nhỏ trên joint 0 — dễ verify bằng mắt trên Polyscope
std::vector<double> q_cmd = q_start;
q_cmd[0] += 0.2 * std::sin(2.0 * M_PI * t / duration_s);
rtde_c.servoJ(q_cmd, 0.0, 0.0, DT, LOOKAHEAD, GAIN);
auto after_servo = Clock::now();
double latency = std::chrono::duration<double, std::milli>(
after_servo - loop_start).count();
std::vector<double> q_actual = rtde_r.getActualQ();
double err = maxError(q_cmd, q_actual);
double ts = std::chrono::duration<double, std::milli>(
after_servo - t_begin).count();
logger.log(ts, q_cmd, q_actual, err, latency);
sum_err += err;
max_lat = std::max(max_lat, latency);
++samples;
// Print mỗi 100 ms
if (samples % 50 == 0) {
std::cout << " t=" << std::fixed << std::setprecision(2) << t
<< "s | err=" << std::setprecision(4) << err
<< " rad | lat=" << std::setprecision(2) << latency << "ms\n";
}
// Timing: ngủ phần còn lại của chu kỳ
auto loop_end = Clock::now();
double used = std::chrono::duration<double>(loop_end - loop_start).count();
if (used < DT)
std::this_thread::sleep_for(std::chrono::duration<double>(DT - used));
t += DT;
}
rtde_c.servoStop();
std::cout << "\n=== ServoJ Summary ===\n"
<< "Samples : " << samples << "\n"
<< "Avg err : " << std::fixed << std::setprecision(5)
<< (sum_err / samples) << " rad\n"
<< "Max lat : " << std::setprecision(2) << max_lat << " ms\n"
<< "Log : " << log_file << "\n";
}
// ── main ──────────────────────────────────────────────────────────────────────
int main(int argc, char* argv[]) {
const std::string ip = (argc > 1) ? argv[1] : "192.168.56.101";
std::cout << "Connecting to " << ip << " ...\n";
RTDEControlInterface rtde_c(ip);
RTDEReceiveInterface rtde_r(ip);
std::cout << "Connected.\n\n";
// 1. Home position — luôn bắt đầu từ đây
std::vector<double> q_home = {0.0, -M_PI/2, 0.0, -M_PI/2, 0.0, 0.0};
executeMoveJ(rtde_c, rtde_r, q_home, 0.5, 0.5); // speed thấp khi test
// 2. Pick position (thay bằng joint angles thực tế cho task của bạn)
std::vector<double> q_pick = {M_PI/4, -M_PI/3, M_PI/2, -M_PI/4, M_PI/2, 0.0};
executeMoveJ(rtde_c, rtde_r, q_pick);
// 3. MoveL — TCP đường thẳng
std::vector<double> pose_above = {0.300, -0.400, 0.500, 0.0, M_PI, 0.0};
executeMoveL(rtde_c, rtde_r, pose_above);
// 4. ServoJ streaming demo (chỉ làm sau khi moveJ/moveL OK)
executeServoJ(rtde_c, rtde_r, 3.0, "servo_log.csv");
// 5. Về home
std::cout << "\nReturning home...\n";
executeMoveJ(rtde_c, rtde_r, q_home, 0.5, 0.5);
rtde_c.disconnect();
rtde_r.disconnect();
std::cout << "Done.\n";
return 0;
}
Bước 7 — Phân tích log tracking error
Sau khi chạy, bạn có file servo_log.csv. Phân tích nhanh bằng Python:
import csv
import statistics
with open("servo_log.csv") as f:
rows = list(csv.DictReader(f))
errors = [float(r["max_err_rad"]) for r in rows]
latencies = [float(r["latency_ms"]) for r in rows]
print(f"Samples : {len(rows)}")
print(f"Error : mean={statistics.mean(errors):.4f} "
f"max={max(errors):.4f} rad "
f"stdev={statistics.stdev(errors):.4f}")
print(f"Latency : mean={statistics.mean(latencies):.2f} "
f"max={max(latencies):.2f} ms")
print(f"\nBenchmark tham khảo:")
print(f" servoJ error tốt : < 0.001 rad (~0.06°)")
print(f" servoJ error chấp : < 0.005 rad (~0.29°)")
print(f" Loop latency mục tiêu: < 2 ms (đúng 500 Hz)")
Dấu hiệu vấn đề khi xem log:
- Latency đột ngột tăng > 5 ms → CPU bị preempt, cân nhắc isolcpus hoặc PREEMPT_RT kernel
- Tracking error tăng dần theo thời gian → GAIN quá thấp hoặc LOOKAHEAD quá dài
- Error spike tại chỗ đổi hướng → robot chạm joint limit, kiểm tra lại workspace
Bước 8 — TCP frames và kiểm tra TCP pose
Khi dùng moveL, cần hiểu rõ TCP frame. UR robot dùng convention:
x, y, z— vị trí TCP (meters) trong base framerx, ry, rz— rotation theo axis-angle (radians), không phải Euler angles
# Kiểm tra TCP frame đang active
tcp = rtde_r.getActualTCPPose()
print(f"TCP: x={tcp[0]:.4f}m y={tcp[1]:.4f}m z={tcp[2]:.4f}m")
print(f" rx={tcp[3]:.4f} ry={tcp[4]:.4f} rz={tcp[5]:.4f} rad")
# Đọc joint torques — phát hiện va chạm
torques = rtde_r.getActualMoment()
print(f"Torques: {[f'{t:.1f}Nm' for t in torques]}")
Bước 9 — Checklist trước khi lên robot thật
Khi code đã test ổn trên URSim, checklist bắt buộc trước khi deploy lên hardware:
- Emergency stop trong tầm tay — không xa hơn 1 bước chân
- Vùng làm việc sạch — không người, không vật cản trong tầm arm
- Speed slider Polyscope ở 10% — test lần đầu với tốc độ thấp nhất
- Program "External Control" đang chạy trên Teach Pendant
- Joint limits check — đảm bảo q_target nằm trong [-2π, 2π]
- Đổi IP từ
192.168.56.101sang IP thật của robot - Verify TCP offset — nếu có tool/gripper, đặt đúng TCP trong Polyscope
- Chạy moveJ home position trước — làm quen với tốc độ và hành trình
# Chạy với robot thật (chỉ đổi IP)
./ur_controller 192.168.1.100 # IP robot thật
Tổng kết series 15 bài
Qua 15 bài, chúng ta đã xây dựng toàn bộ stack robot arm controller từ nền tảng đến production:
| Nhóm | Bài | Nội dung |
|---|---|---|
| Nền tảng | 1–5 | FK/IK, Jacobian, C++ setup, robot model, IK solver |
| Motion primitives | 6–12 | Frames, MoveJ, MoveL, MoveC, blending, S-curve profiles |
| Advanced | 13–14 | Jogging & servoJ, motion planning tránh va chạm (MoveIt2/OMPL) |
| Real robot | 15 | ros2_control + ur_rtde + logging (bài này) |
Điểm khác biệt giữa kỹ sư robotics giỏi và trung bình không nằm ở việc biết thêm thuật toán — mà ở simulation-first mindset, logging đủ để debug khi robot hành xử lạ, và hiểu từng lớp abstraction từ joint command xuống đến metal. Ba thứ đó bạn đã có sau series này.
Bài viết liên quan
- Motion planning tránh va chạm: MoveIt2 & OMPL — bài trước: plan trajectory tránh obstacle
- Jogging và servo control: jog khớp và twist TCP — servoJ trong bối cảnh jogging
- FK/IK và Jacobian: nền tảng toán học robot arm — quay lại nền tảng nếu cần ôn
