程序结构分析

1. 初始化和设置

ros::init(argc, argv, "moveit_pose_demo");
moveit::planning_interface::MoveGroupInterface arm("panda_arm");

• 初始化 ROS 节点
• 创建 MoveGroupInterface 连接到 Panda 机械臂规划组

2. 参数配置

arm.allowReplanning(true);                    // 允许重新规划
arm.setGoalPositionTolerance(0.02);           // 位置容忍误差 2cm
arm.setGoalOrientationTolerance(0.03);        // 姿态容忍误差 0.03弧度
arm.setMaxVelocityScalingFactor(0.1);         // 最大速度比例因子 10%

3. 目标姿态设置

target_pose.pose.position.x = 0.3;           // 目标位置 (0.3, 0.3, 0.6)
target_pose.pose.position.y = 0.3;
target_pose.pose.position.z = 0.6;
quaternion.setRPY(0, 3.1415, 0);             // 欧拉角转四元数 (Roll=0, Pitch=π, Yaw=0)

4. 运动执行流程

1. 规划到初始目标位置

   • 设置起始状态为当前状态
   • 规划路径并检查是否成功
   • 执行规划

2. 相对运动

   target_pose.pose.position.x += 0.2;       // 在当前位置基础上 X 方向移动 20cm
   arm.move();                               // 直接规划并运动
   

3. 移动到预设位置

   arm.setNamedTarget("ready");              // 使用预定义的 "ready" 姿态
   arm.move();
   

关键特点

1. 安全设置：使用 10% 的最大速度，适合演示环境
2. 容错机制：允许重新规划，提高运动成功率
3. 多种运动方式：
   • 精确位姿规划 (setPoseTarget + plan + execute)
   • 相对运动 (move)
   • 预设姿态 (setNamedTarget)

---

目标姿态设置详解

坐标系基础

在 MoveIt 中，目标姿态是相对于规划参考系 (Planning Frame) 的。从代码中可以看到：

std::string planning_frame = arm.getPlanningFrame();
target_pose.header.frame_id = planning_frame;

这个规划参考系通常是：

• 机器人基坐标系 (base_link 或 panda_link0)
• 或者世界坐标系 (world)

位置设置 (Position)

target_pose.pose.position.x = 0.3;  // X方向 0.3米
target_pose.pose.position.y = 0.3;  // Y方向 0.3米  
target_pose.pose.position.z = 0.6;  // Z方向 0.6米

这些坐标值是相对于规划参考系原点的绝对位置。

以 Panda 机械臂为例：

• 原点通常在机器人底座中心
• X轴：向前
• Y轴：向左
• Z轴：向上

姿态设置 (Orientation)

欧拉角表示法

tf2::Quaternion quaternion;
quaternion.setRPY(0, 3.1415, 0);  // Roll=0, Pitch=π, Yaw=0

RPY 参数解释：

• Roll (滚转)：绕 X 轴旋转
• Pitch (俯仰)：绕 Y 轴旋转
• Yaw (偏航)：绕 Z 轴旋转

这里的 (0, 3.1415, 0) 表示：

• 不绕 X 轴旋转
• 绕 Y 轴旋转 180° (π ≈ 3.1415 弧度)
• 不绕 Z 轴旋转

四元数转换

target_pose.pose.orientation.x = quaternion.x();
target_pose.pose.orientation.y = quaternion.y();
target_pose.pose.orientation.z = quaternion.z();
target_pose.pose.orientation.w = quaternion.w();

相对位置设置

代码中展示了两种设置方式：

1. 绝对位置设置

// 直接设置绝对坐标
target_pose.pose.position.x = 0.3;
target_pose.pose.position.y = 0.3;
target_pose.pose.position.z = 0.6;

2. 相对当前位置设置

// 获取当前位置
geometry_msgs::PoseStamped current_pose = arm.getCurrentPose();

// 基于当前位置进行偏移
target_pose = current_pose;
target_pose.pose.position.x += 0.2;  // 在X方向移动20cm

实际应用示例

示例1：垂直向下的末端执行器

// 末端执行器垂直向下
quaternion.setRPY(0, M_PI, 0);  // Pitch=180°

示例2：水平向前的末端执行器

// 末端执行器水平向前
quaternion.setRPY(0, M_PI/2, 0);  // Pitch=90°

示例3：自定义姿态

// 45度倾斜
quaternion.setRPY(M_PI/4, M_PI/2, 0);  // Roll=45°, Pitch=90°

重要注意事项

1. 工作空间限制：设置的位置必须在机器人工作空间内
2. 奇异性避免：某些姿态可能导致机械臂处于奇异位置
3. 碰撞检测：MoveIt 会自动检测与环境或其他连杆的碰撞
4. 逆运动学解：确保目标姿态有可行的逆运动学解

调试技巧

// 打印当前位姿进行调试
geometry_msgs::PoseStamped current_pose = arm.getCurrentPose();
ROS_INFO("Current position: x=%.3f, y=%.3f, z=%.3f", 
         current_pose.pose.position.x,
         current_pose.pose.position.y, 
         current_pose.pose.position.z);

通过理解这些概念，你可以精确控制机械臂末端执行器到达任何期望的位置和姿态。