机器人 / 系统课时

添加末端姿态路径约束

添加末端姿态路径约束

当机械臂端部托着托盘或夹持易洒物品时,只让末端在终点摆正还不够:中途大幅倾斜也会造成问题。本课以 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++ 可执行程序的功能包:

bash
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(),不会驱动真实机器人。

cpp
#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
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 和关节状态发布运行:

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

预期结果:RViz 中出现 Panda 机械臂,MoveIt 的规划服务可用;启动日志和加载时间因环境不同而异。若教程包安装在其他工作区,应先 source 该工作区的 install/setup.bash。

在终端 B 构建并运行本例:

bash
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

预期输出(两次均规划成功时,仅摘出本程序的对比行):

text
[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。放宽后成功只说明可行域变大,不表示可忽略托盘的实际倾斜上限。

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

目录与阅读设置

阅读设置

17

记录一个想法

添加末端姿态路径约束

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

登录哪都通