机器人 / 系统课时
添加末端姿态路径约束
添加末端姿态路径约束
当机械臂端部托着托盘或夹持易洒物品时,只让末端在终点摆正还不够:中途大幅倾斜也会造成问题。本课以 ROS 2 Jazzy、MoveIt 2 和 Panda 机械臂为例,对同一个目标分别进行无约束和姿态受限规划,查看轨迹姿态变化与规划结果。
学习目标
- 区分只约束终点的目标约束与约束沿途状态的路径约束。
- 使用
OrientationConstraint指定参考坐标系、末端连杆和角度容差。 - 对比规划成败及轨迹最大姿态偏移,并定位常见无解原因。
目标约束与路径约束
setPoseTarget() 指定期望终点位姿;例如位置误差容许 0.01 m、终点姿态误差容许 0.05 rad,只能保证规划终点满足相应要求,并不限制中间关节姿态。setPathConstraints() 则把 Constraints 消息加入规划请求,要求路径上的有效状态同时满足姿态约束、关节限位和碰撞检查。

不要把 setGoalOrientationTolerance() 当作路径约束:它调整的是终点姿态容差。反过来,只有路径约束而没有可达目标,也无法生成有意义的轨迹。规划器按轨迹离散状态进行有效性检查,实际执行仍需考虑控制误差。
设定参考姿态与容差
moveit_msgs::msg::OrientationConstraint 采用单位四元数表示期望朝向。为避免凭空猜测机械臂起始姿态,本例读取当前末端位姿,直接以当前四元数作为约束参考;目标仅沿参考坐标系的 Y 方向移动 0.10 m,不改变目标朝向。
| 字段 | 本例取值 | 实际作用 |
|---|---|---|
header.frame_id |
当前末端位姿的参考坐标系 | 说明目标四元数相对于哪个坐标系 |
link_name |
getEndEffectorLink() 的返回值 |
受约束的末端连杆 |
orientation |
当前末端四元数 | 沿途期望朝向 |
parameterization |
ROTATION_VECTOR |
以旋转向量解释三个轴的姿态误差 |
absolute_x/y/z_axis_tolerance |
均为 0.15 rad |
三轴各自允许的绝对旋转误差,约 8.6° |
weight |
1.0 |
约束权重字段,不能代替容差判定 |
三个轴的容差不是末端位置的 XYZ 距离,也不等于一个总旋转角必须小于 0.15 rad。如果任务允许绕竖直轴转动,可以适当放宽对应轴,但必须结合参考坐标系检查实际含义。0.05 rad 的严约束比 0.30 rad 更容易遇到无解或规划超时。
用 MoveGroupInterface 对照规划
前提是 Ubuntu 24.04、ROS 2 Jazzy、MoveIt 2 和 Panda 演示配置已可用,且 moveit2_tutorials 能被 ROS 2 找到。以下工作区示例为 ~/ws_moveit,按本机已有工作区调整。先创建带 C++ 可执行程序的功能包:
source /opt/ros/jazzy/setup.bash
mkdir -p ~/ws_moveit/src
cd ~/ws_moveit/src
ros2 pkg create --build-type ament_cmake --license Apache-2.0 --dependencies rclcpp moveit_ros_planning_interface moveit_msgs --node-name compare_constraints pose_constraint_demo预期结果:生成 pose_constraint_demo/package.xml、CMakeLists.txt 和 src/compare_constraints.cpp,其中已声明相关依赖。将 src/compare_constraints.cpp 替换为下列完整程序。两次规划共用当前起点和目标,但都只调用 plan(),不会驱动真实机器人。
#include <algorithm>
#include <memory>
#include <thread>
#include <Eigen/Geometry>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit_msgs/msg/orientation_constraint.hpp>
#include <moveit_msgs/msg/constraints.hpp>
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<rclcpp::Node>(
"panda_orientation_demo",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
std::thread spinner([&executor] { executor.spin(); });
int result = 0;
{
using MoveGroup = moveit::planning_interface::MoveGroupInterface;
MoveGroup arm(node, "panda_arm");
arm.setPlanningTime(15.0);
arm.setNumPlanningAttempts(5);
const auto initial = arm.getCurrentState(5.0);
const auto tip = arm.getEndEffectorLink();
if (!initial || tip.empty()) {
RCLCPP_ERROR(node->get_logger(), "Missing robot state or end-effector link");
result = 1;
} else {
const auto start = arm.getCurrentPose(tip);
arm.setPoseReferenceFrame(start.header.frame_id);
arm.setStartStateToCurrentState();
auto goal = start.pose;
goal.position.y += 0.10; // 同一个位姿目标,两种规划条件
arm.setGoalPositionTolerance(0.01);
arm.setGoalOrientationTolerance(0.05);
arm.setPoseTarget(goal, tip);
// 用轨迹关节角回代正运动学,测量末端最大总旋转角。
auto max_deviation_deg = [&](const MoveGroup::Plan& plan) {
moveit::core::RobotState state(*initial);
const Eigen::Quaterniond q0(
state.getGlobalLinkTransform(tip).rotation());
double largest = 0.0;
for (const auto& point : plan.trajectory.joint_trajectory.points) {
state.setVariablePositions(
plan.trajectory.joint_trajectory.joint_names, point.positions);
state.update();
const Eigen::Quaterniond q(
state.getGlobalLinkTransform(tip).rotation());
largest = std::max(largest, q0.angularDistance(q));
}
return largest * 180.0 / 3.141592653589793;
};
MoveGroup::Plan free_plan;
bool free_ok =
arm.plan(free_plan) == moveit::core::MoveItErrorCode::SUCCESS;
if (free_ok)
RCLCPP_INFO(node->get_logger(), "free=SUCCESS max_dev_deg=%.2f",
max_deviation_deg(free_plan));
else
RCLCPP_WARN(node->get_logger(), "free=FAILED");
moveit_msgs::msg::OrientationConstraint oc;
oc.header.frame_id = start.header.frame_id;
oc.link_name = tip;
oc.orientation = start.pose.orientation;
oc.parameterization =
moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;
oc.absolute_x_axis_tolerance = 0.15;
oc.absolute_y_axis_tolerance = 0.15;
oc.absolute_z_axis_tolerance = 0.15;
oc.weight = 1.0;
moveit_msgs::msg::Constraints path;
path.orientation_constraints.push_back(oc);
arm.setPathConstraints(path);
MoveGroup::Plan constrained_plan;
bool constrained_ok =
arm.plan(constrained_plan) == moveit::core::MoveItErrorCode::SUCCESS;
if (constrained_ok)
RCLCPP_INFO(node->get_logger(), "constrained=SUCCESS max_dev_deg=%.2f",
max_deviation_deg(constrained_plan));
else
RCLCPP_WARN(node->get_logger(), "constrained=FAILED");
arm.clearPathConstraints(); // 防止约束影响后续规划请求
arm.clearPoseTargets();
}
}
executor.cancel();
spinner.join();
rclcpp::shutdown();
return result;
}预期结果:完成构建并运行后,程序将对比两种规划的成败和轨迹离散点最大末端旋转角;不调用 execute(),不会驱动机器人。下面还需配置构建文件并启动 MoveIt。
把功能包中的 CMakeLists.txt 改为以下完整内容;package.xml 使用创建功能包时生成的依赖声明即可。
cmake_minimum_required(VERSION 3.8)
project(pose_constraint_demo)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_msgs REQUIRED)
add_executable(compare_constraints src/compare_constraints.cpp)
ament_target_dependencies(compare_constraints
rclcpp moveit_ros_planning_interface moveit_msgs)
install(TARGETS compare_constraints DESTINATION lib/${PROJECT_NAME})
ament_package()预期结果:CMake 将 compare_constraints 编译为 ROS 2 可执行节点,安装在功能包对应的 lib 目录;这里的 CMake 是构建配置,不产生独立的控制台输出。
运行、验证与无解处理
在终端 A 启动 Panda 的 MoveIt 演示场景,保持 move_group 和关节状态发布运行:
source /opt/ros/jazzy/setup.bash
ros2 launch moveit2_tutorials move_group.launch.py预期结果:RViz 中出现 Panda 机械臂,MoveIt 的规划服务可用;启动日志和加载时间因环境不同而异。若教程包安装在其他工作区,应先 source 该工作区的 install/setup.bash。
在终端 B 构建并运行本例:
source /opt/ros/jazzy/setup.bash
cd ~/ws_moveit
colcon build --packages-select pose_constraint_demo
source install/setup.bash
ros2 run pose_constraint_demo compare_constraints预期输出(两次均规划成功时,仅摘出本程序的对比行):
[INFO] [<TIME>] [panda_orientation_demo]: free=SUCCESS max_dev_deg=<ANGLE>
[INFO] [<TIME>] [panda_orientation_demo]: constrained=SUCCESS max_dev_deg=<ANGLE><TIME> 和 <ANGLE> 是随运行变化的时间与角度数值,并非程序原样打印的文本;若无法规划,对应行会是 free=FAILED 或 constrained=FAILED。max_dev_deg 是轨迹离散点相对初始姿态的最大总旋转角,用于两组对比,不是分别计算 X、Y、Z 旋转向量误差的严格合规性证明。
验证时固定起始关节状态和目标,将路径容差依次改为 0.30、0.15、0.05 rad,每组在同一环境下运行 10 次,记录成功次数、耗时及最大偏转。例如成功次数为 9/10 和 5/10,成功率分别为 90% 与 50%;这只是计算方式,不是保证得到的实际结果。无约束与有约束都可能规划成功且偏转相近,不能据此认定约束未生效。
若添加约束后失败,依次检查:/joint_states 是否持续更新;起点是否已满足参考姿态;header.frame_id 与 link_name 是否存在且对应正确;目标是否可达、无碰撞;最后再尝试放宽容差至 0.30 rad 或将规划时间由 15 s 提到 30 s。放宽后成功只说明可行域变大,不表示可忽略托盘的实际倾斜上限。