Integration: ROS 2 MoveIt2, URScript và Real Robot Deploy

Đây là bài cuối cùng trong series Traditional Manipulation Control. Qua 7 bài trước, chúng ta đã xây dựng kiến thức từ kinematics cơ bản → trajectory types → MoveJ/MoveL → MoveC → blending → motion profiles → spline & TOPPRA.

Bây giờ đến lúc tổng hợp tất cả vào một workflow thực tế: thiết kế trajectory trong Python → gửi đến MoveIt2 → thực thi trên robot UR5e thật qua ros2_control.

Architecture tổng quan

Python Script (trajectory design)
    │
    ▼
MoveIt2 (planning + collision checking)
    │
    ▼
ros2_control (hardware abstraction)
    │
    ▼
UR Driver (ur_robot_driver)
    │
    ▼
UR5e Controller (Real Robot)

Các thành phần chính

Cài đặt môi trường ROS 2

# ROS 2 Humble (Ubuntu 22.04)
sudo apt install ros-humble-moveit
sudo apt install ros-humble-ur-robot-driver
sudo apt install ros-humble-ros2-controllers

# Workspace
mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src

# Clone UR driver
git clone -b humble https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git

# Build
cd ~/ros2_ws
colcon build --symlink-install
source install/setup.bash

MoveIt2 — Planning Framework

OMPL Planners vs Pilz Industrial Planner

MoveIt2 Python API

#!/usr/bin/env python3
"""
MoveIt2 trajectory planning example.
Requires ROS 2 Humble + MoveIt2 + ur_robot_driver.
"""
import rclpy
from rclpy.node import Node
from moveit_msgs.msg import (
    MotionPlanRequest,
    Constraints,
    JointConstraint,
    PositionConstraint,
    OrientationConstraint,
)
from moveit_msgs.srv import GetMotionPlan
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import JointState
import numpy as np


class TrajectoryPlanner(Node):
    def __init__(self):
        super().__init__('trajectory_planner')

        # MoveIt2 planning service
        self.plan_client = self.create_client(
            GetMotionPlan,
            '/plan_kinematic_path'
        )
        self.plan_client.wait_for_service(timeout_sec=10.0)

        self.get_logger().info('TrajectoryPlanner ready')

    def plan_joint_goal(self, joint_names, joint_values,
                        planner_id='PTP', velocity_scaling=0.5):
        """
        Plan MoveJ (joint space) trajectory.

        Args:
            joint_names: list of joint names
            joint_values: target joint angles (rad)
            planner_id: 'PTP' (Pilz) or 'RRTstar' (OMPL)
            velocity_scaling: 0.0 to 1.0
        """
        request = MotionPlanRequest()
        request.group_name = 'ur_manipulator'
        request.planner_id = planner_id
        request.num_planning_attempts = 5
        request.allowed_planning_time = 5.0
        request.max_velocity_scaling_factor = velocity_scaling
        request.max_acceleration_scaling_factor = velocity_scaling

        # Joint constraints (target)
        constraints = Constraints()
        for name, value in zip(joint_names, joint_values):
            jc = JointConstraint()
            jc.joint_name = name
            jc.position = value
            jc.tolerance_above = 0.001
            jc.tolerance_below = 0.001
            jc.weight = 1.0
            constraints.joint_constraints.append(jc)

        request.goal_constraints.append(constraints)

        # Call planning service
        future = self.plan_client.call_async(
            GetMotionPlan.Request(motion_plan_request=request)
        )
        rclpy.spin_until_future_complete(self, future)

        response = future.result()
        if response.motion_plan_response.error_code.val == 1:
            self.get_logger().info('Planning succeeded!')
            return response.motion_plan_response.trajectory
        else:
            self.get_logger().error(
                f'Planning failed: {response.motion_plan_response.error_code.val}'
            )
            return None

    def plan_cartesian_goal(self, target_pose, planner_id='LIN',
                            velocity_scaling=0.3):
        """
        Plan MoveL (Cartesian space) trajectory.

        Args:
            target_pose: PoseStamped
            planner_id: 'LIN' (Pilz) for straight line
        """
        request = MotionPlanRequest()
        request.group_name = 'ur_manipulator'
        request.planner_id = planner_id
        request.num_planning_attempts = 5
        request.allowed_planning_time = 5.0
        request.max_velocity_scaling_factor = velocity_scaling
        request.max_acceleration_scaling_factor = velocity_scaling

        # Position constraint
        constraints = Constraints()
        pc = PositionConstraint()
        pc.header = target_pose.header
        pc.link_name = 'tool0'
        pc.weight = 1.0
        constraints.position_constraints.append(pc)

        # Orientation constraint
        oc = OrientationConstraint()
        oc.header = target_pose.header
        oc.link_name = 'tool0'
        oc.orientation = target_pose.pose.orientation
        oc.absolute_x_axis_tolerance = 0.001
        oc.absolute_y_axis_tolerance = 0.001
        oc.absolute_z_axis_tolerance = 0.001
        oc.weight = 1.0
        constraints.orientation_constraints.append(oc)

        request.goal_constraints.append(constraints)

        future = self.plan_client.call_async(
            GetMotionPlan.Request(motion_plan_request=request)
        )
        rclpy.spin_until_future_complete(self, future)

        response = future.result()
        if response.motion_plan_response.error_code.val == 1:
            self.get_logger().info('Cartesian planning succeeded!')
            return response.motion_plan_response.trajectory
        else:
            self.get_logger().error('Cartesian planning failed!')
            return None

Pilz Industrial Motion Planner

Pilz planner cung cấp deterministic MoveJ, MoveL, MoveC — giống hệt robot controller truyền thống:

# Pilz LIN (MoveL) — Straight line in Cartesian
def plan_movel(planner, target_pose, velocity=0.1):
    """Plan MoveL using Pilz LIN planner."""
    return planner.plan_cartesian_goal(
        target_pose,
        planner_id='LIN',
        velocity_scaling=velocity / 0.25  # Normalize to max
    )

# Pilz PTP (MoveJ) — Joint space point-to-point
def plan_movej(planner, joint_values, velocity=1.0):
    """Plan MoveJ using Pilz PTP planner."""
    joint_names = [
        'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
        'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
    ]
    return planner.plan_joint_goal(
        joint_names, joint_values,
        planner_id='PTP',
        velocity_scaling=velocity / 3.14
    )

# Pilz CIRC (MoveC) — Circular arc
# Requires via point specification in MoveIt2 constraints

URScript — Direct Robot Control

Khi cần kiểm soát trực tiếp không qua MoveIt2, URScript là lựa chọn:

#!/usr/bin/env python3
"""
Send URScript commands directly to UR robot via ROS 2.
"""
import rclpy
from rclpy.node import Node
from std_msgs.msg import String


class URScriptSender(Node):
    def __init__(self):
        super().__init__('urscript_sender')
        self.publisher = self.create_publisher(
            String,
            '/urscript_interface/script_command',
            10
        )

    def send_movej(self, joints, a=1.4, v=1.05, r=0):
        """Send MoveJ via URScript."""
        joints_str = ', '.join([f'{j:.4f}' for j in joints])
        script = f'movej([{joints_str}], a={a}, v={v}, r={r})'
        msg = String()
        msg.data = script
        self.publisher.publish(msg)
        self.get_logger().info(f'Sent: {script}')

    def send_movel(self, pose, a=1.2, v=0.25, r=0):
        """Send MoveL via URScript."""
        pose_str = ', '.join([f'{p:.4f}' for p in pose])
        script = f'movel(p[{pose_str}], a={a}, v={v}, r={r})'
        msg = String()
        msg.data = script
        self.publisher.publish(msg)
        self.get_logger().info(f'Sent: {script}')

    def send_movec(self, via_pose, end_pose, a=1.2, v=0.1, r=0):
        """Send MoveC via URScript."""
        via_str = ', '.join([f'{p:.4f}' for p in via_pose])
        end_str = ', '.join([f'{p:.4f}' for p in end_pose])
        script = f'movec(p[{via_str}], p[{end_str}], a={a}, v={v}, r={r})'
        msg = String()
        msg.data = script
        self.publisher.publish(msg)

    def send_pick_and_place(self, pick_pose, place_pose,
                             approach_height=0.1):
        """Complete pick-and-place with URScript."""
        # Approach pick
        approach = list(pick_pose)
        approach[2] += approach_height
        self.send_movel(approach, v=0.25)

        # Move down to pick
        self.send_movel(pick_pose, v=0.05)

        # Close gripper
        msg = String()
        msg.data = 'set_digital_out(0, False)'
        self.publisher.publish(msg)

        # Retreat
        self.send_movel(approach, v=0.25)

        # Move to place (MoveJ for speed)
        place_approach = list(place_pose)
        place_approach[2] += approach_height
        # Use IK on robot side
        script = f'movej(get_inverse_kin(p[{", ".join([f"{p:.4f}" for p in place_approach])}]), a=1.4, v=1.05)'
        msg.data = script
        self.publisher.publish(msg)

        # Place
        self.send_movel(place_pose, v=0.05)

        # Open gripper
        msg.data = 'set_digital_out(0, True)'
        self.publisher.publish(msg)

        # Retreat
        self.send_movel(place_approach, v=0.25)

ros2_control — Hardware Abstraction

follow_joint_trajectory Action

Cách chuẩn để thực thi trajectory trên robot thật:

#!/usr/bin/env python3
"""
Execute trajectory via follow_joint_trajectory action.
"""
import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from builtin_interfaces.msg import Duration
import numpy as np


class TrajectoryExecutor(Node):
    def __init__(self):
        super().__init__('trajectory_executor')
        self.action_client = ActionClient(
            self,
            FollowJointTrajectory,
            '/scaled_joint_trajectory_controller/follow_joint_trajectory'
        )
        self.action_client.wait_for_server(timeout_sec=10.0)

    def execute_trajectory(self, joint_names, positions, velocities,
                           timestamps):
        """
        Execute a trajectory on the robot.

        Args:
            joint_names: ['shoulder_pan_joint', ...]
            positions: list of joint position arrays
            velocities: list of joint velocity arrays
            timestamps: list of timestamps (seconds)
        """
        goal = FollowJointTrajectory.Goal()
        goal.trajectory.joint_names = joint_names

        for pos, vel, t in zip(positions, velocities, timestamps):
            point = JointTrajectoryPoint()
            point.positions = pos.tolist()
            point.velocities = vel.tolist()
            point.time_from_start = Duration(
                sec=int(t),
                nanosec=int((t % 1) * 1e9)
            )
            goal.trajectory.points.append(point)

        # Send 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 False

        self.get_logger().info('Trajectory executing...')

        # Wait for result
        result_future = goal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, result_future)

        result = result_future.result().result
        if result.error_code == FollowJointTrajectory.Result.SUCCESSFUL:
            self.get_logger().info('Trajectory completed successfully!')
            return True
        else:
            self.get_logger().error(f'Trajectory failed: {result.error_string}')
            return False

Complete Workflow: Design → Plan → Execute

#!/usr/bin/env python3
"""
Complete workflow: Design trajectory in Python → Execute on UR5e.
"""
import rclpy
import numpy as np
from scipy.interpolate import CubicSpline

# Step 1: Design trajectory (offline, in Python)
waypoints = np.array([
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],       # Home
    [np.pi/6, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, 0],  # Approach
    [np.pi/6, -np.pi/3, np.pi/3, -np.pi/2, np.pi/3, 0],  # Pick
    [np.pi/6, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, 0],  # Retreat
    [np.pi/2, -np.pi/4, np.pi/4, -np.pi/3, np.pi/3, 0],  # Transport
    [np.pi/2, -np.pi/3, np.pi/3, -np.pi/2, np.pi/3, 0],  # Place
    [0, -np.pi/3, np.pi/3, -np.pi/2, np.pi/2, 0],        # Home
])

# Timing for each segment
segment_times = [1.0, 0.5, 0.3, 0.5, 1.0, 0.5, 1.0]
t_waypoints = np.cumsum([0] + segment_times)

# Step 2: Cubic spline interpolation
dt = 0.008  # 125 Hz (UR controller rate)
t_dense = np.arange(0, t_waypoints[-1], dt)

joint_names = [
    'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
    'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]

positions = np.zeros((len(t_dense), 6))
velocities = np.zeros((len(t_dense), 6))

for j in range(6):
    cs = CubicSpline(t_waypoints, waypoints[:, j], bc_type='clamped')
    positions[:, j] = cs(t_dense)
    velocities[:, j] = cs(t_dense, 1)

# Step 3: Execute on robot
def main():
    rclpy.init()
    executor = TrajectoryExecutor()

    success = executor.execute_trajectory(
        joint_names=joint_names,
        positions=positions,
        velocities=velocities,
        timestamps=t_dense.tolist()
    )

    if success:
        print("Pick-and-place cycle completed!")
    else:
        print("Execution failed — check robot status")

    executor.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Safety — An toàn khi deploy

Velocity và Acceleration Limits

# Safety limits cho UR5e
UR5E_LIMITS = {
    'joint_velocity': [2.094, 2.094, 3.142, 3.142, 3.142, 3.142],  # rad/s
    'joint_acceleration': [10.0] * 6,  # rad/s² (reduced for safety)
    'tcp_velocity': 1.0,    # m/s (cobot mode)
    'tcp_force': 150,       # N (collision detection)
}

def validate_trajectory(positions, velocities, accelerations, dt, limits):
    """Validate trajectory against safety limits."""
    violations = []

    for i, t in enumerate(np.arange(0, len(positions) * dt, dt)):
        # Check joint velocities
        for j in range(6):
            if abs(velocities[i, j]) > limits['joint_velocity'][j]:
                violations.append(
                    f"t={t:.3f}: Joint {j+1} velocity "
                    f"{velocities[i, j]:.3f} exceeds limit "
                    f"{limits['joint_velocity'][j]:.3f}"
                )

        # Check joint accelerations
        if i > 0:
            for j in range(6):
                if abs(accelerations[i, j]) > limits['joint_acceleration'][j]:
                    violations.append(
                        f"t={t:.3f}: Joint {j+1} acceleration "
                        f"{accelerations[i, j]:.3f} exceeds limit"
                    )

    if violations:
        print(f"SAFETY: {len(violations)} violations found!")
        for v in violations[:5]:  # Print first 5
            print(f"  {v}")
        return False
    else:
        print("SAFETY: All limits satisfied")
        return True

Workspace Monitoring

# Check if trajectory stays within safe workspace
def check_workspace(robot, positions, workspace_bounds):
    """
    Verify trajectory stays within defined workspace.

    workspace_bounds: dict with 'x_min', 'x_max', 'y_min', etc.
    """
    for i, q in enumerate(positions):
        T = robot.fkine(q)
        x, y, z = T.t

        if (x < workspace_bounds['x_min'] or x > workspace_bounds['x_max'] or
            y < workspace_bounds['y_min'] or y > workspace_bounds['y_max'] or
            z < workspace_bounds['z_min'] or z > workspace_bounds['z_max']):
            print(f"WARNING: Point {i} at [{x:.3f}, {y:.3f}, {z:.3f}] "
                  f"outside workspace!")
            return False

    print("Workspace check: OK")
    return True

Launch Files

# launch/ur5e_moveit.launch.py
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    ur_driver_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory('ur_robot_driver'),
                'launch', 'ur_control.launch.py'
            )
        ),
        launch_arguments={
            'ur_type': 'ur5e',
            'robot_ip': '192.168.1.100',
            'launch_rviz': 'true',
        }.items()
    )

    moveit_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(
                get_package_share_directory('ur_moveit_config'),
                'launch', 'ur_moveit.launch.py'
            )
        ),
        launch_arguments={
            'ur_type': 'ur5e',
        }.items()
    )

    return LaunchDescription([
        ur_driver_launch,
        moveit_launch,
    ])

Quy trình deploy thực tế

Checklist trước khi chạy trên robot thật

1. Simulation test — Chạy toàn bộ trajectory trong MuJoCo hoặc Gazebo
2. Validate limits — Kiểm tra velocity, acceleration, workspace
3. Dry run — Chạy ở tốc độ 10% (velocity_scaling=0.1)
4. Collision check — MoveIt2 collision detection với scene objects
5. Emergency stop — Luôn có E-stop trong tầm tay
6. Teach pendant — Sẵn sàng override bằng teach pendant

Workflow tóm tắt

1. Design trajectory (Python + numpy/scipy)
    ↓
2. Validate (check limits, workspace, singularity)
    ↓
3. Simulate (MuJoCo/Gazebo — verify visually)
    ↓
4. MoveIt2 plan (collision checking, optimization)
    ↓
5. Execute at 10% speed (safety dry run)
    ↓
6. Execute at full speed (production)

Tài liệu tham khảo

• Coleman, D. et al. "Reducing the Barrier to Entry of Complex Robotic Software: a MoveIt! Case Study." Journal of Software Engineering for Robotics, 2014. arXiv:1404.3785
• Pilz GmbH. Pilz Industrial Motion Planner for MoveIt. GitHub
• Scherzinger, S. et al. "Forward-Looking Integration of ros2_control for Real-Time Robot Control." ROSCon, 2022. ros2_control Docs
• Universal Robots. ROS 2 Driver Documentation. GitHub

Kết luận Series

Qua 8 bài, series Traditional Manipulation Control đã cover toàn bộ kiến thức cần thiết:

Đây là nền tảng vững chắc để tiến lên các kỹ thuật hiện đại: AI-based manipulation, imitation learning, và diffusion policy.

Bài viết liên quan

• Grasping và Manipulation cơ bản — Bước tiếp theo: AI manipulation
• ROS 2 Control Hardware Interface — Chi tiết ros2_control
• MoveIt2 Motion Planning — MoveIt2 configuration chi tiết