机器人 / 系统课时

规划关节目标的无碰撞路径

规划关节目标的无碰撞路径

本课以 ROS 2 Jazzy 和 MoveIt 2 的 Panda 七轴机械臂为例,从实时关节状态构造规划起点,修改一个关节得到目标配置,再生成并预览一条经过关节限位与碰撞检查的轨迹。示例只规划,不向控制器下发运动。

学习目标

  • 区分当前关节状态、目标关节配置与规划轨迹。
  • 使用 MoveGroupInterface 生成 Panda 机械臂的关节空间路径。
  • 通过 RViz、终端输出和反例辨认越限与碰撞失败。

关节目标如何进入规划器

Panda 的 panda_arm 包含 panda_joint1 至 panda_joint7 七个转动关节,角度单位为弧度。示例读取当前七关节角度作为目标的初始副本,只把 panda_joint1 改为 -1.0。这比把其余关节随意设为 0 更稳妥:例如 panda_joint4 的合法范围约为 -3.0718 至 -0.0698 弧度,0 本身就越限。

setJointValueTarget() 设置目标并返回关节限位检查结果;它不会证明目标不碰撞。plan() 才会结合机器人模型、当前 Planning Scene 中的自碰撞与外部障碍信息,搜索从起点到目标的可行轨迹。即使首尾姿态都有效,中间轨迹仍可能撞到障碍。

关节目标、模型约束和规划场景如何共同决定路径

准备 Panda 规划服务

以下操作假设 Ubuntu 24.04、ROS 2 Jazzy,且 ~/ws_moveit2 已构建包含 moveit2_tutorials 与 Panda 配置的 MoveIt 工作空间。第一个终端启动带 RViz、关节状态发布和 move_group 服务的演示环境:

bash
source /opt/ros/jazzy/setup.bash
source ~/ws_moveit2/install/setup.bash
ros2 launch moveit2_tutorials move_group.launch.py

预期结果:RViz 中显示 Panda,规划服务启动且机器人关节状态持续更新;此终端保持运行。若工作空间不在该路径,请替换为实际路径。

第二个终端创建独立 C++ 包。ros2 pkg create 会生成 package.xml;后续只需替换 CMakeLists.txt 并添加两个源文件。

bash
source /opt/ros/jazzy/setup.bash
source ~/ws_moveit2/install/setup.bash
cd ~/ws_moveit2/src
ros2 pkg create joint_goal_demo --build-type ament_cmake --dependencies rclcpp moveit_ros_planning_interface moveit_core moveit_msgs launch launch_ros moveit_configs_utils
mkdir -p joint_goal_demo/src joint_goal_demo/launch

预期结果:出现 joint_goal_demo/package.xml、joint_goal_demo/CMakeLists.txt、src 和 launch 目录;包已声明程序编译与启动所需依赖。

编写关节规划节点

将以下完整程序保存为 joint_goal_demo/src/plan_joint_goal.cpp。后台执行器负责接收当前关节状态;规划时选择当前状态为起点,输出规划点数并通过 /display_planned_path 发布轨迹,不调用 execute()。

cpp
#include <chrono>
#include <thread>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit/robot_state/robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto opts = rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true);
  auto node = rclcpp::Node::make_shared("joint_goal_planner", opts);
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  std::thread spinner([&executor]() { executor.spin(); });

  const bool ok = [&]() -> bool {
    moveit::planning_interface::MoveGroupInterface arm(node, "panda_arm");
    auto state = arm.getCurrentState(10.0);
    if (!state) {
      RCLCPP_ERROR(node->get_logger(), "No current joint state");
      return false;
    }

    // 保留当前其余六个关节,避免凭空构造越限目标。
    const auto* group = state->getJointModelGroup("panda_arm");
    std::vector<double> q;
    state->copyJointGroupPositions(group, q);
    if (q.size() != 7) return false;
    q[0] = -1.0;  // panda_joint1,单位 rad

    arm.setStartStateToCurrentState();
    if (!arm.setJointValueTarget(q)) {
      RCLCPP_ERROR(node->get_logger(), "Joint target out of bounds");
      return false;
    }
    arm.setPlanningTime(5.0);
    arm.setNumPlanningAttempts(5);
    arm.setMaxVelocityScalingFactor(0.1);
    arm.setMaxAccelerationScalingFactor(0.1);

    moveit::planning_interface::MoveGroupInterface::Plan plan;
    if (arm.plan(plan) != moveit::core::MoveItErrorCode::SUCCESS ||
        plan.trajectory_.joint_trajectory.points.empty()) {
      RCLCPP_ERROR(node->get_logger(), "Planning failed");
      return false;
    }

    // 将成功轨迹送往 RViz,不触发执行控制器。
    auto pub = node->create_publisher<moveit_msgs::msg::DisplayTrajectory>(
        "/display_planned_path", rclcpp::QoS(1).transient_local());
    moveit_msgs::msg::DisplayTrajectory display;
    display.model_id = arm.getRobotModel()->getName();
    display.trajectory_start = plan.start_state_;
    display.trajectory.push_back(plan.trajectory_);
    pub->publish(display);
    RCLCPP_INFO(node->get_logger(), "PLAN_OK points=%zu",
                plan.trajectory_.joint_trajectory.points.size());
    std::this_thread::sleep_for(std::chrono::seconds(3));
    return true;
  }();

  executor.cancel();
  spinner.join();
  rclcpp::shutdown();
  return ok ? 0 : 1;
}

预期行为:起点来自实时状态,目标只更改第一关节;限位不合格时直接拒绝,规划成功时打印点数并发布轨迹。setMaxVelocityScalingFactor(0.1) 与加速度缩放 0.1 都表示使用配置上限的 10%,不是碰撞安全系数。

构建并预览路径

用下列内容覆盖 joint_goal_demo/CMakeLists.txt,将可执行程序与启动文件安装进 ROS 2 包:

cmake
cmake_minimum_required(VERSION 3.8)
project(joint_goal_demo)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_msgs REQUIRED)
add_executable(plan_joint_goal src/plan_joint_goal.cpp)
ament_target_dependencies(plan_joint_goal
  rclcpp moveit_ros_planning_interface moveit_core moveit_msgs)
install(TARGETS plan_joint_goal DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
ament_package()

预期行为:编译链接 MoveIt 规划接口,并使 ros2 launch 可以发现该包的启动文件。

将下列完整启动文件保存为 joint_goal_demo/launch/plan_joint_goal.launch.py。ROS 2 无全局参数服务器,规划节点需要单独收到 Panda 的 URDF、SRDF 和运动学参数。

python
from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
    config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs()
    return LaunchDescription([
        Node(
            package="joint_goal_demo",
            executable="plan_joint_goal",
            output="screen",
            parameters=[
                config.robot_description,
                config.robot_description_semantic,
                config.robot_description_kinematics,
            ],
        )
    ])

预期行为:节点启动时能找到 panda_arm 规划组,并连接第一个终端中的 move_group。

保持首个终端运行,在第二个终端构建并启动:

bash
cd ~/ws_moveit2
colcon build --packages-select joint_goal_demo
source install/setup.bash
ros2 launch joint_goal_demo plan_joint_goal.launch.py

预期输出包含下面这条日志,实际时间与轨迹点数不同:

text
[INFO] [<TIME>] [joint_goal_planner]: PLAN_OK points=<POINT_COUNT>

在 RViz 的 MotionPlanning 显示项中检查 Planned Path,确保轨迹话题为 /display_planned_path,查看轨迹动画。成功标志是关节轨迹点数大于零,机械臂沿可见路径到达目标;本例没有执行动作,真实关节状态不会因这段程序改变。

区分越限与碰撞导致的失败

先将程序中的 q[0] = -1.0 恢复,确保基线能规划;再尝试把目标设置中的第四关节改为 q[3] = 0.0 并重新编译运行。Panda 第四关节不允许此值,预期出现 Joint target out of bounds,程序在调用规划器之前停止。这说明目标合法性检查与整条路径的碰撞检测是两回事。

若目标未越限但报告 Planning failed,按顺序检查:RViz 中的起点与目标是否发生自碰撞;Planning Scene 是否含阻断路径的箱体;/joint_states 是否持续发布且时间戳新鲜;规划时间 5.0 秒是否过短。可以先移除测试障碍或增大规划时间至 10.0 秒,再与原结果对比,不能仅靠提高速度缩放来修复规划失败。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

规划关节目标的无碰撞路径

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通