机器人 / 系统课时

用 C++ 设置目标并调用 plan

用 C++ 设置目标并调用 plan

在 Panda 机械臂的 MoveIt 2 场景中,分别提交关节角度目标和末端位姿目标,读取规划是否成功以及返回的轨迹点数。整个示例只调用 plan(),不会驱动机械臂,便于先验证起点、目标和规划场景是否一致。

学习目标

  • 使用 MoveGroupInterface 从当前状态构造关节目标。
  • 用 setPoseTarget() 指定末端位姿,并区分关节目标与位姿目标。
  • 检查 plan() 的返回码和 Plan 中的轨迹,不把规划成功当成执行完成。

准备 Panda 规划环境

示例适用于 Ubuntu 24.04、ROS 2 Jazzy 和已配置 Panda 的 MoveIt 2。需要 ROS 2 C++ 开发工具、moveit_ros_planning_interface,以及能启动 move_group、机器人状态发布与关节状态的 Panda 演示配置。先检查依赖:

bash
source /opt/ros/jazzy/setup.bash
ros2 pkg prefix moveit_ros_planning_interface
ros2 pkg prefix moveit2_tutorials

预期输出:

text
<PATH>
<PATH>

<PATH> 是安装前缀,可能位于 /opt/ros/jazzy 或工作空间的 install 目录;第二条包查询失败时,须先安装教程包,或改用已有的 panda_moveit_config 演示启动文件。

创建独立的 CMake 软件包,命令会自动生成依赖声明与可执行目标:

bash
mkdir -p ~/ws_moveit/src
cd ~/ws_moveit/src
ros2 pkg create moveit_plan_targets --build-type ament_cmake --dependencies rclcpp geometry_msgs moveit_ros_planning_interface --node-name plan_targets

预期结果:生成 moveit_plan_targets/src/plan_targets.cpp、CMakeLists.txt 和 package.xml;下文直接替换生成的 .cpp 文件内容。

理解起点与规划请求的关系

MoveGroupInterface 是 C++ 请求入口,move_group 负责调用规划流水线。getCurrentState() 从状态监视器取得关节状态;setStartStateToCurrentState() 指定本次规划起点;目标设置接口决定终点条件;plan() 返回 MoveGroupInterface::Plan,其中 trajectory 是候选轨迹。

C++ 关节目标与位姿目标的规划数据流

两次规划都从实际当前状态出发:第一次得到的关节轨迹没有执行,不能直接把它的终点当成第二次的真实起点。位姿目标的坐标取自规划参考系,姿态四元数必须有效。

编写两类目标的完整程序

将以下代码保存为 ~/ws_moveit/src/moveit_plan_targets/src/plan_targets.cpp。panda_arm 是 Panda 示例的规划组;其他机器人需要替换为自己 SRDF 中的组名,并调整关节数量和目标位姿。

cpp
#include <geometry_msgs/msg/pose.hpp>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit/robot_state/robot_state.hpp>
#include <rclcpp/rclcpp.hpp>

#include <thread>
#include <vector>

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

  // 状态监视器需要持续处理 ROS 2 回调。
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  std::thread spinner([&executor]() { executor.spin(); });

  int status = 0;
  {
    using moveit::planning_interface::MoveGroupInterface;
    MoveGroupInterface arm(node, "panda_arm");
    arm.setPlanningTime(5.0);
    arm.setMaxVelocityScalingFactor(0.1);
    arm.setMaxAccelerationScalingFactor(0.1);

    auto current = arm.getCurrentState(10.0);
    if (!current) {
      RCLCPP_ERROR(logger, "No current robot state");
      status = 1;
    } else {
      const auto* group = current->getJointModelGroup("panda_arm");
      if (!group) {
        RCLCPP_ERROR(logger, "Planning group not found");
        status = 1;
      } else {
        std::vector<double> joints;
        current->copyJointGroupPositions(group, joints);
        joints.at(0) = -0.4;  // Panda 第一个关节,单位为弧度。

        arm.setStartStateToCurrentState();
        if (arm.setJointValueTarget(joints)) {
          MoveGroupInterface::Plan joint_plan;
          if (arm.plan(joint_plan) == moveit::core::MoveItErrorCode::SUCCESS) {
            RCLCPP_INFO(logger, "Joint plan OK, points=%zu",
                        joint_plan.trajectory.joint_trajectory.points.size());
          } else {
            RCLCPP_ERROR(logger, "Joint plan failed");
            status = 1;
          }
        } else {
          RCLCPP_ERROR(logger, "Joint target violates limits");
          status = 1;
        }

        // 上一次仅规划未执行,所以再次取实际当前状态作为起点。
        arm.setStartStateToCurrentState();
        arm.setPoseReferenceFrame(arm.getPlanningFrame());
        geometry_msgs::msg::Pose goal;
        goal.orientation.w = 1.0;  // 单位四元数。
        goal.position.x = 0.28;
        goal.position.y = -0.20;
        goal.position.z = 0.50;

        if (arm.setPoseTarget(goal)) {
          MoveGroupInterface::Plan pose_plan;
          if (arm.plan(pose_plan) == moveit::core::MoveItErrorCode::SUCCESS) {
            RCLCPP_INFO(logger, "Pose plan OK, points=%zu",
                        pose_plan.trajectory.joint_trajectory.points.size());
          } else {
            RCLCPP_ERROR(logger, "Pose plan failed");
            status = 1;
          }
        } else {
          RCLCPP_ERROR(logger, "Pose target rejected");
          status = 1;
        }
      }
    }
  }

  executor.cancel();
  spinner.join();
  rclcpp::shutdown();
  return status;
}

预期结果:程序对关节目标与末端位姿目标分别发送一次规划请求;可达且无碰撞时,两次各得到一条含关节轨迹点的 Plan。程序没有调用 execute(),机械臂不会因为本程序而运动。

编译、运行与验收

在终端 A 编译软件包,确保新终端已经加载 ROS 2 环境:

bash
source /opt/ros/jazzy/setup.bash
cd ~/ws_moveit
colcon build --packages-select moveit_plan_targets
source install/setup.bash

预期输出包含:

text
Summary: 1 package finished [<TIME>]

<TIME> 为实际耗时。然后在终端 B 启动 Panda 演示,保持窗口运行:

bash
source /opt/ros/jazzy/setup.bash
source ~/ws_moveit/install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py

预期结果:RViz 显示 Panda,move_group 已启动,关节状态持续更新。若使用自行生成的 panda_moveit_config,将命令中的包名改为它,保留 demo.launch.py。

在终端 A 运行自己的节点:

bash
ros2 run moveit_plan_targets plan_targets

规划成功时应看到以下关键日志形态(运行时还会有 MoveIt 内部日志):

text
[INFO] [<TIME>] [plan_targets]: Joint plan OK, points=<N>
[INFO] [<TIME>] [plan_targets]: Pose plan OK, points=<N>

<N> 是实际轨迹点数,受规划器、起始关节状态和场景影响,并非固定数值。可验收的结果是两次状态均成功、轨迹点数大于零,且没有执行动作;只调用 plan() 不会自动在 RViz 播放这两条轨迹,需要另外发布可视化消息才会显示规划动画。

规划失败时优先检查什么

  • 出现 No current robot state:检查演示程序是否正常发布 /joint_states、节点是否在旋转处理回调,仿真时间配置是否一致;可执行 ros2 topic hz /joint_states 检查消息频率,正常应持续显示非零频率。
  • 出现 Planning group not found 或机器人模型加载错误:核对 panda_arm 与 SRDF 规划组,确认 robot_description 和 robot_description_semantic 已正确提供。
  • 关节目标越限:先检查 joints.at(0) = -0.4 对目标机器人的限位是否有效;不要将角度值误当成弧度,也不要在 setJointValueTarget() 返回 false 后继续规划。
  • 位姿规划失败:检查 arm.getPlanningFrame()、末端链路、位置单位米和单位四元数;确认目标位姿有 IK 解并且路径无碰撞。可先把规划时间由 5.0 秒提高到 10.0 秒排除采样不足,但超时延长不能修复不可达目标。

读取返回值时应先判断 MoveItErrorCode::SUCCESS,再使用 Plan::trajectory。规划轨迹是候选结果,不代表执行器已经跟踪完成,真实硬件应在碰撞场景、控制器及安全限位独立确认后再考虑执行。

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

目录与阅读设置

阅读设置

17

记录一个想法

用 C++ 设置目标并调用 plan

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

登录哪都通