机器人 / 系统课时
用 PoseStamped 指定末端位姿目标
用 PoseStamped 指定末端位姿目标
同一组位置数值如果换了参考坐标系,就可能指向完全不同的空间位置。这里以 ROS 2 Jazzy、MoveIt 2 的 Panda 机械臂为例,构造带坐标系的 PoseStamped,给末端链接设置目标,完成一次只规划、不执行的验证。
学习目标
- 正确填写
PoseStamped的参考帧、位置和四元数。 - 使用 MoveGroupInterface 为指定末端链接提交位姿目标并检查规划结果。
- 判断坐标系、姿态约束和 TF 问题为何导致不同的逆运动学结果。
消息字段与目标链接
PoseStamped 由 header 和 pose 组成。header.frame_id 指定位置与姿态相对于哪个坐标系表达;pose.position 的单位是米,pose.orientation 是顺序为 x、y、z、w 的四元数。示例的 position=(0.28,-0.20,0.50) 表示在 panda_link0 基座坐标系下的目标位置,orientation=(0,0,0,1) 表示与该参考帧没有相对旋转。
另一个容易混淆的名字是被约束的机器人链接。调用 setPoseTarget(goal, "panda_link8") 时,panda_link8 是要抵达目标位姿的链接,而不是坐标系的来源。若改成夹爪的工具中心链接,同一位姿数值会要求不同的关节配置。这里使用 Panda 模型中实际存在的 panda_link8,不假定它等于夹持点 TCP。
参考帧如何影响路径
规划时,MoveIt 需要先理解目标位姿相对于机器人规划坐标系的位置,再寻找让 panda_link8 抵达该位姿的关节配置,并在关节限位与规划场景的碰撞约束下搜索轨迹。setPoseTarget 返回成功仅表示目标被接受,并不表示逆运动学或路径规划已经成功。

例如,若某参考帧相对 panda_link0 仅沿 X 正方向平移 0.10 m 且不旋转,那么该帧下的 x=0.28 m 对应基座下 x=0.38 m。只修改 frame_id 而不重新计算数值,目标实体位置就变了;要保持同一目标,必须连同坐标一起做 TF 变换。
在 Panda 演示中提交目标
以下使用 Ubuntu 24.04、ROS 2 Jazzy 和已构建的 moveit2_tutorials 工作区,约定路径为 ~/ws_moveit2。工作区还应包含 Panda 的 MoveIt 配置。先生成一个含 C++ 可执行节点的包;ros2 pkg create 会同时生成构建文件和依赖声明。
source /opt/ros/jazzy/setup.bash
cd ~/ws_moveit2/src
ros2 pkg create pose_goal_demo --build-type ament_cmake --dependencies rclcpp geometry_msgs moveit_ros_planning_interface --node-name pose_goal预期结果:创建 pose_goal_demo/src/pose_goal.cpp、CMakeLists.txt 和 package.xml。如果包已经存在,不要重复创建,直接修改源文件。
将 pose_goal_demo/src/pose_goal.cpp 完整替换为以下代码:
#include <memory>
#include <thread>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <rclcpp/rclcpp.hpp>
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>(
"pose_goal",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
std::thread worker([&executor] { executor.spin(); });
{
moveit::planning_interface::MoveGroupInterface arm(node, "panda_arm");
arm.setPlanningTime(10.0);
arm.setGoalPositionTolerance(0.01);
arm.setGoalOrientationTolerance(0.05);
geometry_msgs::msg::PoseStamped goal;
goal.header.frame_id = "panda_link0"; // 在机器人基座坐标系中表达目标
goal.header.stamp = node->now();
goal.pose.position.x = 0.28;
goal.pose.position.y = -0.20;
goal.pose.position.z = 0.50;
goal.pose.orientation.w = 1.0; // 单位四元数,不能全部填 0
// 指定被约束的末端链接;只提交目标,不会立即求出轨迹
if (!arm.setPoseTarget(goal, "panda_link8")) {
RCLCPP_ERROR(node->get_logger(), "pose target rejected");
} else {
moveit::planning_interface::MoveGroupInterface::Plan plan;
const bool success =
arm.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS;
RCLCPP_INFO(node->get_logger(), "plan_success=%s, points=%zu",
success ? "true" : "false",
plan.trajectory.joint_trajectory.points.size());
}
}
executor.cancel();
worker.join();
rclcpp::shutdown();
return 0;
}预期结果:此代码调用 plan() 生成关节轨迹,但不调用 execute(),因此不会发出执行命令。0.01 是 1 cm 的位置目标容差,0.05 是约 2.9° 的姿态目标容差;这两项是规划目标容差,并非控制器的执行误差阈值。
在终端 A 启动 Panda 演示,等待 move_group 与关节状态正常就绪:
source /opt/ros/jazzy/setup.bash
source ~/ws_moveit2/install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py预期结果:RViz 打开 Panda 演示场景,MotionPlanning 插件可以访问规划组 panda_arm。
在终端 B 构建并运行:
source /opt/ros/jazzy/setup.bash
cd ~/ws_moveit2
colcon build --packages-select pose_goal_demo
source install/setup.bash
ros2 run pose_goal_demo pose_goal预期输出(无遮挡且 IK、规划均成功时的日志关键内容):
plan_success=true, points=<COUNT><COUNT> 是轨迹点数,随起始关节状态与规划结果改变;日志另有带动态时间的前缀。失败则为 plan_success=false,不能把接受位姿目标等同于规划成功。
验证坐标系与排查失败
先确认 TF 树中确实存在基座与末端链接间的变换:
ros2 run tf2_ros tf2_echo panda_link0 panda_link8预期结果:持续打印当前末端相对基座的平移和四元数;这些值来自当前机器人状态,不是程序设置的目标值。按 Ctrl+C 结束观察。若 TF 不可用,先检查机器人模型、robot_state_publisher 和 /joint_states。
可再做一组对照:把源代码中的 goal.header.frame_id 改为 panda_link1,保留所有位置与姿态数值,重新构建运行。由于 panda_link1 相对基座存在关节变换,目标实际位置通常发生变化,IK 和规划结果也可能不同;这并非规划器忽略了输入,而是目标的参考帧变了。
当目标能正确变换但规划失败时,按“目标是否在工作空间内、四元数是否归一化、姿态是否可达、场景是否存在碰撞、起始关节状态是否新鲜”的顺序排查。即使成功规划,关节空间中的无碰撞轨迹也不保证末端沿直线到达目标。