机器人 / 系统课时
按笛卡尔路点生成直线运动轨迹
按笛卡尔路点生成直线运动轨迹
机械臂末端需要先抬起、再沿水平方向平移时,普通关节目标规划不保证工具端沿直线移动。这里以 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 逐点求逆运动学并检查状态是否满足约束。末端路径近似直线,不代表七个关节角度也线性变化;一旦某个采样点无逆解或碰撞,结果可能只包含起点到失败点之前的有效前缀。

选定插值与跳变判据
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++ 包:
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:
#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 和运动学参数:
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 找到:
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})生效结果:重新构建后,install/cartesian_demo/share/cartesian_demo/launch/ 中应有该启动文件;保留自动生成的目标、依赖和可执行文件安装规则。
运行、判读与失败修正
先在终端 A 启动包含 move_group、关节状态和 RViz 的 Panda 教程场景:
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 构建并启动自己的节点:
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> 和数值占位的实际值依初始姿态与场景变化):
[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,应重新选起点或路点,避免仅为通过检查而调大告警值。
本例仅对轨迹采样点做检查,不能证明采样点之间绝对无碰撞;用于实机前,还须校验轨迹时间参数化、速度和加速度限值、控制器约束,并重新核对当前机器人状态与规划场景。