Motion Planning Tránh Va Chạm: MoveIt2 & OMPL

Ở bài 13 về Jogging và Servo Control, chúng ta đã điều khiển robot theo thời gian thực từng khớp một. Nhưng jogging thủ công có một điểm yếu lớn: người vận hành phải tự tránh vật cản. Bài này giải quyết vấn đề đó — motion planning tự động tránh va chạm bằng MoveIt2 và OMPL.

Thay vì tính tay, ta để thuật toán tìm đường đi qua không gian khớp, tự động né hết mọi vật cản. Kết quả: bạn chỉ cần nói "đi đến tọa độ X" và nhấn execute.

1. Tại sao cần Motion Planning tránh va chạm?

Hãy hình dung robot hàn trong xưởng: băng chuyền di chuyển, cột trụ cố định, dây cáp lủng lẳng. Jogging thủ công qua môi trường đó không chỉ chậm mà còn cực kỳ nguy hiểm.

Motion planning giải quyết câu hỏi nền tảng: "Từ cấu hình A đến cấu hình B, không chạm vào vật nào?"

Pipeline hoàn chỉnh:

Start config → [Planner tìm C_free path] → Goal config
                      ↑
              Collision checker kiểm tra từng bước

2. Configuration Space (C-space) — Nền tảng toán học

Trước khi đào sâu code, ta cần hiểu C-space — khái niệm mà mọi motion planner đều dựa vào.

2.1 C-space là gì?

Robot 6-DOF (6 khớp) có C-space 6 chiều. Mỗi điểm trong không gian này là một vector:

q = [θ₁, θ₂, θ₃, θ₄, θ₅, θ₆]

đại diện cho một cấu hình đầy đủ của robot — biết q là biết tất cả.

C-space chia thành hai vùng:

• C_free: Cấu hình robot KHÔNG va chạm với bất kỳ vật nào
• C_collision: Cấu hình robot ĐÂM vào vật cản (bàn, tường, tự va chạm)

Workspace (3D)                  C-space (6D)
─────────────────────────       ───────────────────────
Robot là vật thể 3D phức tạp    Robot là MỘT ĐIỂM duy nhất
Vật cản là hình khối 3D         Vật cản là VÙNG "forbidden"
Đường đi = trajectory phức tạp  Đường đi = đường thẳng trong C_free

Ý tưởng thiên tài: Thay vì tính collision cho từng link robot tại mỗi bước, ta biến đổi bài toán thành việc tìm đường giữa hai điểm trong C_free. Và sampling-based planners làm chính xác điều này.

2.2 Tại sao sampling-based?

Tính C_free chính xác (explicit) cho robot 6-DOF là bài toán NP-hard — không thể làm thực tế. Thay vào đó, sampling-based planners lấy mẫu ngẫu nhiên rồi kiểm tra từng mẫu có collision-free không. Probabilistically complete: nếu có lời giải, thuật toán sẽ tìm ra (với đủ thời gian).

3. Sampling-based Planners — RRT, RRT-Connect, PRM

OMPL cung cấp hàng chục thuật toán. Ba loại quan trọng nhất:

3.1 RRT — Rapidly-Exploring Random Tree

RRT xây một cây từ điểm start, mở rộng ngẫu nhiên về phía goal:

Mỗi iteration:
1. q_rand  = sample_random_in_C_space()   // điểm ngẫu nhiên
2. q_near  = nearest_node_in_tree(q_rand) // node gần nhất trong cây
3. q_new   = step_toward(q_near, q_rand)  // di chuyển một bước nhỏ
4. if collision_free(q_near → q_new):
       tree.add_node(q_new)
5. if q_new ∈ goal_region: SUCCESS

Ưu điểm: Đơn giản, hoạt động tốt với không gian cao chiều. Nhược điểm: Không optimal, đường đi có thể "zig-zag" không cần thiết.

3.2 RRT-Connect — Default của MoveIt2

Cải tiến lớn: thay vì 1 cây từ start, dùng 2 cây — một từ start, một từ goal. Hai cây lớn song song và cố tình kết nối nhau.

Tree_A (từ start) ──────────────► [MEET] ◄──────────────── Tree_B (từ goal)

Lý do là default: RRT-Connect nhanh hơn RRT đơn thuần 5-10x vì hai cây "tìm nhau" thay vì một cây lang thang mù quáng.

3.3 PRM — Probabilistic Roadmap Method

PRM là multi-query planner: xây dựng một bản đồ (roadmap) một lần, dùng lại cho nhiều query.

Phase 1 — Build:
  Lấy mẫu ngẫu nhiên nhiều điểm trong C_free
  Kết nối các điểm gần nhau → graph (roadmap)

Phase 2 — Query (mỗi lần cần plan):
  Kết nối start + goal vào roadmap có sẵn
  Tìm đường ngắn nhất trên graph (A* hoặc Dijkstra)

Khi nào dùng PRM? Robot hàn lặp lại 100 lần/ngày trong môi trường cố định. Build roadmap 1 lần tốn vài giây, sau đó mỗi query chỉ mất mili-giây.

So sánh nhanh

4. Setup Package — CMakeLists.txt và package.xml

4.1 Cấu trúc package

my_moveit_pkg/
├── CMakeLists.txt
├── package.xml
├── src/
│   └── motion_planner.cpp
└── scripts/
    └── motion_planner.py

4.2 CMakeLists.txt

cmake_minimum_required(VERSION 3.8)
project(my_moveit_pkg)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(shape_msgs REQUIRED)

add_executable(motion_planner src/motion_planner.cpp)
target_include_directories(motion_planner PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_features(motion_planner PUBLIC c_std_99 cxx_std_17)
ament_target_dependencies(motion_planner
  rclcpp
  moveit_core
  moveit_ros_planning
  moveit_ros_planning_interface
  geometry_msgs
  moveit_msgs
  shape_msgs
)

install(TARGETS motion_planner
  DESTINATION lib/${PROJECT_NAME})

install(PROGRAMS scripts/motion_planner.py
  DESTINATION lib/${PROJECT_NAME})

ament_package()

Lưu ý quan trọng: ament_package() phải là lệnh cuối cùng trong CMakeLists.txt. Nó thu thập toàn bộ thông tin đã khai báo trước đó để đăng ký package với ament index.

4.3 package.xml

<?xml version="1.0"?>
<package format="3">
  <name>my_moveit_pkg</name>
  <version>0.0.1</version>
  <description>Motion planning tránh va chạm với MoveIt2</description>
  <maintainer email="[email protected]">Tên Bạn</maintainer>
  <license>Apache-2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>moveit_core</depend>
  <depend>moveit_ros_planning</depend>
  <depend>moveit_ros_planning_interface</depend>
  <depend>geometry_msgs</depend>
  <depend>moveit_msgs</depend>
  <depend>shape_msgs</depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

4.4 Build với colcon

cd ~/ros2_ws
colcon build --packages-select my_moveit_pkg --symlink-install
source install/setup.bash

Flag --symlink-install tạo symlink thay vì copy file — Python scripts thay đổi không cần rebuild. Cho MoveIt2, nên thêm --parallel-workers 2 nếu RAM < 16GB vì một số package MoveIt rất nặng.

5. MoveGroupInterface C++ — Lập kế hoạch tránh vật cản

Đây là phần trọng tâm: dùng MoveGroupInterface để thêm vật cản, plan, và execute.

#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <shape_msgs/msg/solid_primitive.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <rclcpp/rclcpp.hpp>
#include <thread>

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<rclcpp::Node>(
        "motion_planner",
        rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
    );
    auto logger = node->get_logger();

    // Spin node trong thread riêng để không block main thread
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    std::thread spinner([&executor]() { executor.spin(); });

    // ============================================================
    // Bước 1: Khởi tạo MoveGroupInterface
    // ============================================================
    const std::string PLANNING_GROUP = "panda_arm";
    moveit::planning_interface::MoveGroupInterface move_group(node, PLANNING_GROUP);
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    // Cache raw pointer để tăng hiệu năng truy cập kinematics
    const moveit::core::JointModelGroup* joint_model_group =
        move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

    RCLCPP_INFO(logger, "Planning frame: %s", move_group.getPlanningFrame().c_str());
    RCLCPP_INFO(logger, "End-effector link: %s", move_group.getEndEffectorLink().c_str());

    // ============================================================
    // Bước 2: Thêm collision object vào planning scene
    // ============================================================
    moveit_msgs::msg::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.getPlanningFrame();
    collision_object.id = "box1";

    // Hình dạng: hộp 5cm × 5cm × 50cm
    shape_msgs::msg::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
    primitive.dimensions[primitive.BOX_X] = 0.05;
    primitive.dimensions[primitive.BOX_Y] = 0.05;
    primitive.dimensions[primitive.BOX_Z] = 0.5;

    // Vị trí đặt hộp phía trước robot
    geometry_msgs::msg::Pose box_pose;
    box_pose.orientation.w = 1.0;
    box_pose.position.x = 0.48;
    box_pose.position.y = 0.0;
    box_pose.position.z = 0.25;

    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
    collision_object.operation = collision_object.ADD;

    planning_scene_interface.addCollisionObjects({collision_object});
    RCLCPP_INFO(logger, "Added 'box1' to planning scene");

    // Chờ scene cập nhật (addCollisionObjects là async)
    rclcpp::sleep_for(std::chrono::seconds(1));

    // ============================================================
    // Bước 3: Plan đường đi tránh hộp
    // ============================================================
    geometry_msgs::msg::Pose target_pose;
    target_pose.orientation.w = 1.0;
    target_pose.position.x = 0.28;
    target_pose.position.y = 0.4;
    target_pose.position.z = 0.5;
    move_group.setPoseTarget(target_pose);

    // Cấu hình planner
    move_group.setPlannerId("RRTConnectkConfigDefault");
    move_group.setPlanningTime(10.0);
    move_group.setMaxVelocityScalingFactor(0.3);
    move_group.setMaxAccelerationScalingFactor(0.3);

    moveit::planning_interface::MoveGroupInterface::Plan plan;
    bool success = (move_group.plan(plan) ==
                    moveit::core::MoveItErrorCode::SUCCESS);

    if (success) {
        RCLCPP_INFO(logger, "Planning thành công! Executing...");
        move_group.execute(plan);
    } else {
        RCLCPP_ERROR(logger, "Planning THẤT BẠI — thử tăng planning time");
    }

    // ============================================================
    // Bước 4: Attach object (mô phỏng grasping)
    // ============================================================
    // Khi attach, MoveIt2 coi vật là một phần của robot
    // → collision check bao gồm cả vật khi plan đường tiếp theo
    move_group.attachObject(collision_object.id, "panda_hand");
    RCLCPP_INFO(logger, "Object attached to gripper");

    geometry_msgs::msg::Pose target_pose2;
    target_pose2.orientation.w = 1.0;
    target_pose2.position.x = -0.3;
    target_pose2.position.y = 0.4;
    target_pose2.position.z = 0.5;
    move_group.setPoseTarget(target_pose2);

    success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
    if (success) move_group.execute(plan);

    // Đặt vật xuống
    move_group.detachObject(collision_object.id);

    rclcpp::shutdown();
    spinner.join();
    return 0;
}

Tại sao cần std::thread spinner? MoveGroupInterface giao tiếp với move_group_node qua ROS topics và services — cần executor đang chạy để nhận/gửi messages. Spin trong thread riêng tránh deadlock khi main thread block ở plan().

addCollisionObjects là async — scene không cập nhật ngay lập tức. Luôn sleep hoặc kiểm tra scene đã có object trước khi plan.

6. RobotModel và RobotState — Truy cập Kinematics trực tiếp

Đôi khi bạn cần tính FK/IK thủ công mà không cần plan. RobotModel + RobotState cho phép điều đó.

#include <moveit/robot_model_loader/robot_model_loader.hpp>
#include <moveit/robot_model/robot_model.hpp>
#include <moveit/robot_state/robot_state.hpp>

// Load kinematic model từ URDF (robot_description parameter)
robot_model_loader::RobotModelLoader model_loader(node, "robot_description");
moveit::core::RobotModelPtr robot_model = model_loader.getModel();
RCLCPP_INFO(logger, "Model frame: %s", robot_model->getModelFrame().c_str());

// RobotState: trạng thái robot tại một thời điểm
moveit::core::RobotState robot_state(robot_model);
const moveit::core::JointModelGroup* jmg =
    robot_model->getJointModelGroup("panda_arm");

// Đặt về default values
robot_state.setToDefaultValues();
robot_state.update();

// Forward Kinematics: tính vị trí end-effector
const Eigen::Isometry3d& eef_tf =
    robot_state.getGlobalLinkTransform("panda_hand");
RCLCPP_INFO(logger, "EEF: x=%.3f y=%.3f z=%.3f",
    eef_tf.translation().x(),
    eef_tf.translation().y(),
    eef_tf.translation().z());

// Inverse Kinematics: tìm joint values cho target pose
geometry_msgs::msg::Pose ik_target;
ik_target.position.x = 0.3;
ik_target.position.y = 0.2;
ik_target.position.z = 0.5;
ik_target.orientation.w = 1.0;

bool ik_found = robot_state.setFromIK(jmg, ik_target, 0.1 /*timeout*/);
if (ik_found) {
    std::vector<double> jvals;
    robot_state.copyJointGroupPositions(jmg, jvals);
    RCLCPP_INFO(logger, "IK found: joint[0]=%.2f rad", jvals[0]);
}

// Tính Jacobian
Eigen::Vector3d ref_point(0, 0, 0);
Eigen::MatrixXd jacobian;
robot_state.getJacobian(jmg,
    robot_state.getLinkModel("panda_hand"),
    ref_point, jacobian);

// Validate joint bounds
robot_state.enforceBounds();
bool valid = robot_state.satisfiesBounds();
RCLCPP_INFO(logger, "State valid: %s", valid ? "YES" : "NO");

Sự khác biệt giữa RobotModel và RobotState:

• RobotModel: cấu trúc bất biến (links, joints, limits từ URDF) — load một lần, dùng mãi
• RobotState: snapshot tại một thời điểm — thay đổi khi robot di chuyển

7. Python với moveit_py — Thử nghiệm nhanh

moveit_py là Python binding chính thức của MoveIt2. Không cần compile, chạy trực tiếp — lý tưởng để thử nghiệm planner và parameters.

#!/usr/bin/env python3
"""motion_planner.py — Demo moveit_py API"""

import rclpy
from moveit.planning import MoveItPy
from geometry_msgs.msg import PoseStamped

def main():
    rclpy.init()
    logger = rclpy.logging.get_logger("moveit_py_demo")

    # Khởi tạo — tự kết nối với move_group_node
    panda = MoveItPy(node_name="motion_planner_py")
    panda_arm = panda.get_planning_component("panda_arm")

    # ─── Cách 1: Plan đến predefined state ───
    panda_arm.set_start_state_to_current_state()
    panda_arm.set_goal_state(configuration_name="ready")
    result = panda_arm.plan()
    if result:
        panda.execute(result.trajectory, blocking=True)
        logger.info("Đã đến 'ready'")

    # ─── Cách 2: Plan đến pose cụ thể ───
    pose_goal = PoseStamped()
    pose_goal.header.frame_id = "panda_link0"
    pose_goal.pose.orientation.w = 1.0
    pose_goal.pose.position.x = 0.28
    pose_goal.pose.position.y = -0.2
    pose_goal.pose.position.z = 0.5

    panda_arm.set_start_state_to_current_state()
    panda_arm.set_goal_state(
        pose_stamped_msg=pose_goal,
        pose_link="panda_link8"
    )

    result = panda_arm.plan()
    if result:
        panda.execute(result.trajectory, blocking=True)
        logger.info("Pose goal reached!")
    else:
        logger.error("Planning thất bại! Thử tăng planning_time.")

    # ─── Cách 3: Multi-pipeline (chạy song song, lấy kết quả nhanh nhất) ───
    from moveit.planning import MultiPipelinePlanRequestParameters
    multi_params = MultiPipelinePlanRequestParameters(
        panda, ["ompl_rrtc", "pilz_lin"]
    )
    result = panda_arm.plan(multi_params)
    if result:
        logger.info(f"Best plan: {result.planner_id}")

    rclpy.shutdown()

if __name__ == "__main__":
    main()

Thêm collision object qua Python:

from moveit_msgs.msg import CollisionObject
from shape_msgs.msg import SolidPrimitive
from geometry_msgs.msg import Pose

# Context manager cho read-write scene access
with panda.get_planning_scene_monitor().read_write() as scene:
    obj = CollisionObject()
    obj.id = "table"
    obj.header.frame_id = "panda_link0"
    obj.operation = CollisionObject.ADD

    primitive = SolidPrimitive()
    primitive.type = SolidPrimitive.BOX
    primitive.dimensions = [0.8, 1.2, 0.05]  # bàn lớn

    pose = Pose()
    pose.position.z = -0.025  # mặt bàn ở z=0
    pose.orientation.w = 1.0

    obj.primitives = [primitive]
    obj.primitive_poses = [pose]
    scene.apply_collision_object(obj)
    scene.current_state.update()

Điểm khác biệt quan trọng: apply_collision_object() trong context manager là synchronous — scene cập nhật ngay khi thoát context. Ngược lại với C++ addCollisionObjects() là async.

8. Cấu hình OMPL Planner trong YAML

Để thay đổi planner và parameters mà không cần sửa code:

# config/ompl_planning.yaml
planning_plugin: ompl_interface/OMPLPlanner
request_adapters:
  - default_planning_request_adapters/ResolveConstraintFrames
  - default_planning_request_adapters/ValidateWorkspaceBounds
  - default_planning_request_adapters/CheckStartStateBounds
  - default_planning_request_adapters/CheckStartStateCollision
response_adapters:
  - default_planning_response_adapters/AddTimeOptimalParameterization
  - default_planning_response_adapters/ValidateSolution
  - default_planning_response_adapters/DisplayMotionPath

panda_arm:
  default_planner_config: RRTConnectkConfigDefault
  planner_configs:
    - RRTConnectkConfigDefault
    - RRTkConfigDefault
    - PRMkConfigDefault

  RRTConnectkConfigDefault:
    type: geometric::RRTConnect
    range: 0.0        # 0 = auto-compute từ joint limits
  RRTkConfigDefault:
    type: geometric::RRT
    range: 0.0
    goal_bias: 0.05   # 5% cơ hội sample thẳng goal
  PRMkConfigDefault:
    type: geometric::PRM
    max_nearest_neighbors: 10

request_adapters và response_adapters là pre/post-processing pipeline:

• CheckStartStateCollision: reject ngay nếu start state đã collision
• AddTimeOptimalParameterization: thêm timestamps tối ưu vận tốc cho trajectory

9. Simulation trong RViz — Workflow khuyên dùng

Luôn test trong RViz trước khi chạy robot thật.

# Terminal 1: Khởi động MoveIt2 với Panda demo
ros2 launch moveit2_tutorials move_group.launch.py

# Terminal 2: Build và chạy node C++
cd ~/ros2_ws
colcon build --packages-select my_moveit_pkg
source install/setup.bash
ros2 run my_moveit_pkg motion_planner

# Terminal 3: Chạy Python script (không cần build)
ros2 run my_moveit_pkg motion_planner.py

Checklist trước khi execute thật:

• Planning thành công trong RViz (trajectory preview màu xanh)
• Đường đi không có đột biến góc lớn (smooth motion)
• Velocity scaling ≤ 0.3 (30% max speed) cho lần đầu tiên
• Collision objects đã được đặt đúng vị trí với thực tế
• Joint limits không bị vượt quá trong suốt trajectory
• Test nhiều lần với start state khác nhau (non-deterministic planner)

10. Debug thường gặp

"Planning failed after Xs":

• Tăng setPlanningTime(30.0) — RRT cần thêm thời gian cho narrow passages
• Kiểm tra goal pose có nằm trong workspace không (FK verify)
• Kiểm tra goal pose không overlap với collision object

"Invalid Motion Plan":

• Start state bị collision — robot đang đứng bên trong một object
• Xóa scene và re-add: planning_scene_interface.removeCollisionObjects({"box1"})

"ABORTED: Solution found but controller failed":

• Trajectory controller timeout — giảm velocity scaling hoặc tăng timeout
• Kiểm tra joint limits trong trajectory message

RRT-Connect vs RRT:*

• RRT-Connect: nhanh, không optimal — dùng cho production
• RRT*: chậm, converge về optimal — dùng khi cần path ngắn nhất

11. Tổng kết

Motion planning tránh va chạm không phải phép màu — đó là C-space + sampling + collision checking lặp đi lặp lại. Luồng chuẩn:

1. Định nghĩa planning scene (addCollisionObjects)
2. Đặt start + goal state (setPoseTarget / setJointValueTarget)
3. plan() → đường đi qua C_free
4. execute() trên robot hoặc simulation
5. Nếu thất bại → tăng planning time, thử planner khác, kiểm tra workspace

Bài tiếp theo (bài 15: Integration và System Testing) sẽ kết hợp toàn bộ kiến thức từ series — từ FK/IK đến jogging đến motion planning — thành một controller hoàn chỉnh có thể deploy thực tế.

Bài viết liên quan

• Bài 5: Inverse Kinematics — Từ vị trí TCP đến joint angles
• Bài 13: Jogging và Servo Control — Điều khiển thời gian thực
• FK/IK và Jacobian — Nền tảng kinematics