机器人 / 系统课时

按笛卡尔路点生成直线运动轨迹

按笛卡尔路点生成直线运动轨迹

机械臂末端需要先抬起、再沿水平方向平移时,普通关节目标规划不保证工具端沿直线移动。这里以 ROS 2 Jazzy、MoveIt 2 和 Panda 机械臂为例,生成分段直线的笛卡尔轨迹,并用完成比例与关节步长判断轨迹是否可用。

学习目标

  • 用多个末端位姿路点调用 MoveGroupInterface::computeCartesianPath。
  • 区分插值步长、关节跳变判据与路径完成比例。
  • 在 Panda 演示环境中生成轨迹,拒绝执行部分完成或异常跳变的结果。

理解路点与直线段

示例从机械臂当前末端位姿 P0 出发,保持原来的四元数朝向,依次构造两个目标:P1 沿规划坐标系的 z 轴上移 0.06 m,P2 从 P1 沿 y 轴负方向移动 0.06 m。因此工具端期望走两段各 6 cm 的直线,而不是从 P0 直接斜走到 P2。

waypoints 只需按顺序放入 P1 和 P2;当前起点由机器人状态提供。每段按距离插值后,MoveIt 逐点求逆运动学并检查状态是否满足约束。末端路径近似直线,不代表七个关节角度也线性变化;一旦某个采样点无逆解或碰撞,结果可能只包含起点到失败点之前的有效前缀。

Panda 笛卡尔路点、插值采样与部分完成路径

选定插值与跳变判据

eef_step=0.01 表示末端平移插值的最大步长为 1 cm,不是机械臂速度。将它降到 0.005 可增加逆解和碰撞采样密度,但计算更多,也不能代替连续碰撞检测。avoid_collisions=true 要求求解时考虑当前 Planning Scene 中的机器人自碰撞和环境碰撞;它不会自动绕过障碍改走曲线。

关节跳变是另一类风险:两个相邻末端点只隔 1 cm,逆解却可能让某个关节突然转过很大角度。旧接口中的相对 jump_threshold=2.0 指相邻关节状态距离相对于平均步间距离的倍数,不是 2 rad;设为 0.0 通常表示禁用相对跳变筛选。注意,部分 Jazzy 版本的 MoveGroupInterface 已将这个形参标记为弃用,兼容重载可能不再把它传入求解请求,不能仅凭传参认定检查已生效(见 Jazzy 参数问题记录)。下面直接对返回的 Panda 七个旋转关节做绝对步间变化检查。

返回值 fraction 在正常情况下位于 0.0~1.0:1.0 表示请求的所有路点段均被覆盖,低于 1.0 表示只有有效前缀,出错还可能返回负值。多段路径的比例不一定等于已行进距离除以总距离,因此不要把 fraction=0.75 理解成执行器已经运行了 75%。

编写 Panda 轨迹检查节点

前置条件为 Ubuntu 24.04、ROS 2 Jazzy,已安装 MoveIt 2、Panda 配置及 moveit2_tutorials 工作区。以下以 ~/ws_moveit 为工作区示例,实际位置不同时替换。先在已加载 ROS 环境的终端创建 C++ 包:

bash
source /opt/ros/jazzy/setup.bash
mkdir -p ~/ws_moveit/src
cd ~/ws_moveit/src
ros2 pkg create --build-type ament_cmake --node-name cartesian_demo --dependencies rclcpp geometry_msgs moveit_msgs moveit_ros_planning_interface cartesian_demo
mkdir -p cartesian_demo/launch

预期结果:生成 cartesian_demo/src/cartesian_demo.cpp、CMakeLists.txt、package.xml 和 launch/ 目录。用下面的完整程序替换 src/cartesian_demo.cpp:

cpp
#include <algorithm>
#include <cmath>
#include <exception>
#include <thread>
#include <vector>
#include <geometry_msgs/msg/pose.hpp>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <rclcpp/rclcpp.hpp>

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

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  std::thread spinning([&executor]() { executor.spin(); });
  int exit_code = 0;

  try {
    moveit::planning_interface::MoveGroupInterface arm(node, "panda_arm");
    if (!arm.getCurrentState(10.0)) {
      RCLCPP_ERROR(node->get_logger(), "No current joint state");
      exit_code = 1;
    } else {
      arm.setPoseReferenceFrame(arm.getPlanningFrame());
      arm.setStartStateToCurrentState();

      // 保持末端朝向,只改变规划坐标系中的位置。
      auto p1 = arm.getCurrentPose().pose;
      p1.position.z += 0.06;
      auto p2 = p1;
      p2.position.y -= 0.06;
      std::vector<geometry_msgs::msg::Pose> waypoints{p1, p2};

      moveit_msgs::msg::RobotTrajectory trajectory;
      const double fraction = arm.computeCartesianPath(
          waypoints, 0.01, trajectory, true);

      // Panda 七个旋转关节逐采样点做绝对跳变预警。
      const auto& points = trajectory.joint_trajectory.points;
      double max_delta = 0.0;
      for (std::size_t i = 1; i < points.size(); ++i) {
        for (std::size_t j = 0; j < points[i].positions.size(); ++j) {
          max_delta = std::max(max_delta, std::abs(
              points[i].positions[j] - points[i - 1].positions[j]));
        }
      }

      RCLCPP_INFO(node->get_logger(),
                  "fraction=%.3f, points=%zu, max_delta=%.3f rad",
                  fraction, points.size(), max_delta);
      if (fraction >= 0.999 && points.size() >= 2 && max_delta <= 0.20) {
        RCLCPP_INFO(node->get_logger(), "CHECK PASS: full path; preview only");
      } else {
        RCLCPP_WARN(node->get_logger(),
                    "CHECK FAIL: partial path or large joint step");
      }
    }
  } catch (const std::exception& e) {
    RCLCPP_ERROR(node->get_logger(), "Planning error: %s", e.what());
    exit_code = 1;
  }

  executor.cancel();
  spinning.join();
  rclcpp::shutdown();
  return exit_code;
}

预期结果:运行后记录 fraction、轨迹点数和最大关节步间变化,并输出 CHECK PASS 或 CHECK FAIL。程序不会调用 execute(),避免把尚未复核的路径直接送往控制器。0.20 rad 是本次 Panda 演示用的单步变化告警值,不是 MoveIt 相对跳变阈值,也不是通用实机安全限值。

在 launch/cartesian_demo.launch.py 中保存完整启动文件;它为独立节点注入机器人模型、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="cartesian_demo",
            executable="cartesian_demo",
            output="screen",
            parameters=[
                config.robot_description,
                config.robot_description_semantic,
                config.robot_description_kinematics,
            ],
        )
    ])

预期结果:通过此启动文件运行节点时,机器人模型、SRDF 和运动学参数会一同传入;若未安装 Panda 配置包,应先补齐依赖。

在自动生成的 CMakeLists.txt 中、ament_package() 之前添加下面的片段示例,使启动文件能被 ros2 launch 找到:

cmake
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

生效结果:重新构建后,install/cartesian_demo/share/cartesian_demo/launch/ 中应有该启动文件;保留自动生成的目标、依赖和可执行文件安装规则。

运行、判读与失败修正

先在终端 A 启动包含 move_group、关节状态和 RViz 的 Panda 教程场景:

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

预期结果:RViz 显示 Panda,终端中的 move_group 已就绪;这里要求教程工作区先前已构建完成。再在终端 B 构建并启动自己的节点:

bash
source /opt/ros/jazzy/setup.bash
cd ~/ws_moveit
colcon build --packages-select cartesian_demo --symlink-install
source install/setup.bash
ros2 launch cartesian_demo cartesian_demo.launch.py

预期结果:构建成功后输出本节点的完成比例与关节步长。路径完整且检查通过时,日志形式如下(<TIME> 和数值占位的实际值依初始姿态与场景变化):

text
[INFO] [<TIME>] [cartesian_demo]: fraction=<FRACTION>, points=<N>, max_delta=<RAD> rad
[INFO] [<TIME>] [cartesian_demo]: CHECK PASS: full path; preview only

如果输出 CHECK FAIL,先查看 fraction:小于 0.999 时,不应把有效前缀当作完整路线执行。将第二个路点的位移由 0.06 m 改为 0.03 m,或检查 Planning Scene 的障碍位置及 IK 可达性;更细的 eef_step 只能改变采样,不能让笛卡尔直线绕障。若比例达到 1.0 但 max_delta 大于 0.20 rad,应重新选起点或路点,避免仅为通过检查而调大告警值。

本例仅对轨迹采样点做检查,不能证明采样点之间绝对无碰撞;用于实机前,还须校验轨迹时间参数化、速度和加速度限值、控制器约束,并重新核对当前机器人状态与规划场景。

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

目录与阅读设置

阅读设置

17

记录一个想法

按笛卡尔路点生成直线运动轨迹

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

登录哪都通