MoveIt 机械臂运动规划

move_group 聚合的能力：
  规划管线   ：规划请求适配器 → 规划器插件 → 响应适配器
  逆解       ：kinematics 插件（KDL / TRAC-IK / IKFast）
  碰撞检测   ：CollisionEnv（FCL/Bullet）+ ACM
  规划场景   ：PlanningSceneMonitor 维护世界状态
  执行       ：把轨迹交给 controller（通常经 ros2_control）
  服务/动作  ：/plan、/execute、/compute_ik、/apply_planning_scene ...

三类外部接口：
  C++ MoveGroupInterface   ：生产代码首选，功能最全
  Python moveit_py         ：脚本与实验
  RViz MotionPlanning 插件 ：可视化调试

PlanningScene 的四个组成部分：
  1. 机器人状态（RobotState）：当前关节角、末端位姿
  2. 机器人模型（RobotModel）：URDF + SRDF 描述的结构
  3. 世界几何（World Geometry）：静态环境 + 动态障碍
  4. 附着物体（Attached Bodies）：机器人抓着的物体

PlanningSceneMonitor 的三种同步方式：
  1. 订阅 /monitored_planning_scene 话题（MonitoringScene）
     —— 来自外部（如感知节点）的场景更新
  2. 订阅 /planning_scene 话题（完整场景替换）
  3. 订阅 /collision_object 与 /attached_collision_object
     —— 增量的碰撞体添加/移除，最常用

// 用 PlanningSceneInterface 增删碰撞体
#include <moveit/planning_scene_interface/planning_scene_interface.h>

moveit::planning_interface::PlanningSceneInterface psi;

moveit_msgs::msg::CollisionObject table;
table.header.frame_id = "world";
table.id = "table";
shape_msgs::msg::SolidPrimitive box;
box.type = shape_msgs::msg::SolidPrimitive::BOX;
box.dimensions = {1.2, 0.8, 0.05};
table.primitives.push_back(box);
geometry_msgs::msg::Pose pose;
pose.position.z = 0.4;
table.primitive_poses.push_back(pose);
table.operation = table.ADD;

psi.applyCollisionObjects({table});          // 加入世界
// psi.removeCollisionObjects({"table"});     // 移除

// 抓取成功后把物体附着到末端连杆
moveit_msgs::msg::AttachedCollisionObject aco;
aco.link_name = "panda_hand";                 // 附着到的连杆
aco.object = box_object;                      // 被附着的物体
aco.touch_links = {"panda_hand", "panda_leftfinger",
                   "panda_rightfinger"};      // 允许接触的连杆
aco.object.operation = aco.object.ADD;

psi.applyAttachedCollisionObject(aco);

附着/脱离的状态机（必须成对且顺序正确）：
  抓取成功 → applyAttachedCollisionObject(ADD)
  放置成功 → applyAttachedCollisionObject(REMOVE) 然后 applyCollisionObject(ADD)
           （物体回到世界上，位置更新为放置点）

错误做法：
  抓取后忘了附着 → 规划器认为臂是空的 → 抬起时撞到刚抓的物体
  放置后忘了从世界移除再重新加 → 物体停留在被抓走时的位置

<!-- SRDF 中的 disable_collisions：ACM 的来源 -->
<disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent"/>
<disable_collisions link1="panda_link0" link2="panda_link2" reason="Never"/>
<disable_collisions link1="panda_link1" link2="panda_link3" reason="Never"/>

ACM 的三类来源：
  1. SRDF 的 disable_collisions（静态，来自 URDF 结构）
  2. 运行时通过 PlanningScene 的 allowed_collision_matrix 动态设置
  3. 附着物体的 touch_links（临时的允许接触对）

收益：
  七轴臂若不配置，需检查 C(7,2)=21 对自碰撞
  配置后通常只剩 3~5 对需要检查，规划提速 2~4 倍
  漏配的代价：规划极慢（每步都做全量自碰撞检查）

// 运行时动态放宽/收紧 ACM（例如允许某两个连杆接触）
collision_detection::AllowedCollisionMatrix acm =
    planning_scene->getAllowedCollisionMatrix();
acm.setEntry("panda_link7", "panda_hand", true);   // true = 允许碰撞
planning_scene->setAllowedCollisionMatrix(acm);

碰撞检测的三层加速：
  层次 1：包围体层次（BVH）
    FCL 默认用 OBB，对凸多面体效果好
    典型：无 BVH 时 100 µs/次 → 有 BVH 时 2 µs/次
  层次 2：连续碰撞检测（CCD）
    离散采样（快，可能漏检）vs 扫掠体（准，慢）
    工程折中：采样间隔 < 最薄障碍厚度的一半
  层次 3：碰撞缓存
    缓存「已知无碰撞的构型」，重复查询直接命中
    MoveIt 的 CollisionEnv 支持缓存，固定工位命中率高

# move_group 的碰撞检测参数
move_group:
  ros__parameters:
    # 连续碰撞检测的最大步长（米），越小越安全越慢
    # 机械臂建议 0.01~0.05
    # 由 planning_pipeline 的 collision_checking 配置
    default_planning_pipeline: ompl
    planning_pipelines:
      pipeline_names: ["ompl", "pilz_industrial_motion_planner"]
ompl:
  ros__parameters:
    planning_plugin: ompl_interface/OMPLPlanner
    request_adapters: >-
      default_planner_request_adapters/AddTimeOptimalParameterization
      default_planner_request_adapters/FixWorkspaceBounds
      default_planner_request_adapters/FixStartStateBounds
      default_planner_request_adapters/FixStartStateCollision

ompl:
  ros__parameters:
    planner_configs:
      RRTConnect:
        type: geometric::RRTConnect
        range: 0.0                    # 0 表示用默认步长
      RRTstar:
        type: geometric::RRTstar
        range: 0.15
        goal_bias: 0.05
      BITstar:
        type: geometric::BITstar
      PRM:
        type: geometric::PRM
    panda_arm:
      default_planner_config: RRTConnect
      planner_configs: ["RRTConnect", "RRTstar", "BITstar", "PRM"]
      projection_evaluator: joints(panda_joint1,panda_joint2)
      longest_valid_segment_fraction: 0.005   # 碰撞检测的离散化步长

规划器选型与规划时间：
  RRTConnect ：双向扩展，首次解最快，工业首选（默认）
  RRTstar    ：渐近最优，路径更短但要更长时间
  BITstar    ：综合最优性与速度，近年推荐
  PRM        ：固定环境多次查询，需预构建路标图
  EST/KPIECE ：特定场景，一般不用

规划时间的设置（关键工程经验）：
  规划时间不是「越长越好」，而是「覆盖 p99 即可」
  机械臂典型：0.5~2.0 s，配合 5 次重试
  p50 通常 20~100 ms，p99 可能 1~2 s，设置要看 p99
  盲目设 10 s 只会让失败场景等更久，不提高成功率

请求适配器（规划前，修正问题）：
  FixStartStateBounds       ：把起始状态夹到关节限位内
  FixStartStateCollision    ：起始状态在碰撞中时，微调到最近的自由构型
  FixStartStatePathConstraints：修正起始状态违反的路径约束
  FixWorkspaceBounds        ：给未定义的工作空间边界补默认值
  ValidateWorkspaceBounds   ：校验工作空间边界合理性
  CheckStartStateBounds     ：只检查不修改（会报错而非静默修正）

响应适配器（规划后，优化轨迹）：
  AddTimeParameterization   ：给几何路径加时间（旧）
  AddTimeOptimalParameterization：TOTG，推荐
  ResolveConstraintFrames   ：解析约束中的坐标系

适配器顺序很重要（默认顺序即推荐顺序）：
  1. 先 Fix（修正）再 Validate（校验）
  2. 时间参数化必须放在最后（前面都是几何层面）
  3. 关闭 FixStartStateCollision 会让「起始状态碰撞」直接失败，
     保留它能让系统更鲁棒，但会掩盖真实的场景错误

// 笛卡尔直线规划：逐点逆解，fraction 是关键指标
std::vector<geometry_msgs::msg::Pose> waypoints = {start, mid, end};
moveit_msgs::msg::RobotTrajectory trajectory;
const double eef_step = 0.01;         // 末端步长 1 cm
const double jump_threshold = 1.5;    // 关节跳变阈值（0 表示禁用，不推荐）
double fraction = arm.computeCartesianPath(
    waypoints, eef_step, jump_threshold, trajectory);
if (fraction < 0.99) {
  RCLCPP_WARN(node->get_logger(),
              "cartesian path only %.1f%% complete", fraction * 100);
  return false;                        // 必须检查，不能直接执行
}

eef_step 的取舍：
  0.005 m ：最平滑，逆解次数最多，慢
  0.01 m  ：推荐起点，平衡
  0.05 m  ：快，但中间可能跳过奇异位形导致跟踪偏差

jump_threshold 的作用：
  检测相邻路径点之间关节角是否发生「非物理的跳变」
  逆解在多解间跳变时，关节角会突然变化，执行时是灾难
  设为 0 会禁用检测，奇异附近可能产生瞬间大幅跳变

MTC 的阶段类型：
  CurrentState        ：捕获当前状态作为起点
  GenerateGrasps      ：生成候选抓取位姿
  ComputeIK           ：对每个候选求逆解
  Connect             ：连接两个阶段（可带碰撞检测）
  MoveTo              ：移动到命名目标
  ModifyPlanningScene ：临时增删场景物体（如打开容器盖）
  AllowCollision      ：临时允许某些碰撞（如插入时贴边）

流水线骨架：
  CurrentState → MoveTo(ready) → Connect(→ 预抓取) → GenerateGrasps
               → ComputeIK → AllowCollision(接近时) → MoveTo(抓取)
               → ModifyPlanningScene(附着物体) → MoveTo(抬起)
               → Connect(→ 放置上方) → MoveTo(放置) → ModifyPlanningScene(脱离)

// 用 MTC 构建一个最小抓取任务
auto node = std::make_shared<rclcpp::Node>("mtc");
auto task = std::make_unique<moveit::task_constructor::Task>();
task->stages()->setName("pick");
task->loadRobotModel(node);
task->setProperty("group", "panda_arm");

auto current = std::make_unique<stages::CurrentState>("current");
task->add(std::move(current));

auto move_to_pick = std::make_unique<stages::MoveTo>("approach", solver);
move_to_pick->setGoal("ready");
task->add(std::move(move_to_pick));

// 计划：MTC 会展开所有阶段的解空间，选择整体代价最小的组合
if (task->plan(5) == moveit::core::MoveItErrorCode::SUCCESS) {
  task->execute(*task->solutions().front());
}

交接的三个检查点：
  1. 起始点匹配
     轨迹第一个点的关节角必须与当前 /joint_states 在容差内
     否则控制器报 "start point not within tolerance"
     对策：规划前先更新 RobotState，或设置合理的起始容差

  2. 时间戳
     轨迹点的时间戳必须单调递增且从当前时刻开始
     规划耗时（如 1 s）会让「从 0 开始」的时间戳过期
     对策：执行前重新做时间参数化，或让控制器接受相对时间

  3. 速度/加速度缩放
     max_velocity_scaling_factor 只缩放速度，不缩放加速度
     时间被拉长后，加速度需求反而可能增加
     对策：同时设置 max_acceleration_scaling_factor

// 规划与执行的缩放因子策略
arm.setMaxVelocityScalingFactor(0.3);      // 调试 0.1~0.2，生产 0.5~0.8
arm.setMaxAccelerationScalingFactor(0.2);  // 通常 ≤ 速度缩放
arm.setPlanningTime(1.0);
arm.setNumPlanningAttempts(5);

// 执行前确认状态已同步（异步执行时尤其重要）
arm.asyncExecute(plan);                    // 或 execute(plan) 阻塞等待

RViz MotionPlanning 插件的关键显示项：
  Planning Scene      ：世界几何、附着物体（先确认场景对不对）
  Planned Path        ：规划出的路径（看是否绕远、是否贴障）
  Start State         ：规划时的起始状态（看是否与真机一致）
  Goal State          ：目标状态
  Collision Contacts  ：碰撞点（诊断「为什么判定碰撞」）
  Trajectory Trail    ：执行轨迹（对比规划与实际）

# 常用诊断命令
ros2 topic echo /monitored_planning_scene --once     # 场景内容
ros2 service call /compute_ik moveit_msgs/srv/GetPositionIK "{...}"
ros2 topic hz /joint_states                          # 状态频率
ros2 run moveit_ros_benchmarks moveit_benchmark      # 规划器基准测试
ros2 param get /move_group robot_description_kinematics.panda_arm.kinematics_solver

回归测试的最小用例集：
  1. 从多个起始位形规划到多个目标位形（覆盖工作空间）
  2. 带障碍物的场景（验证碰撞规避）
  3. 抓取与放置全流程（验证附着物体的增删）
  4. 逆解边界位形（验证 IK 插件成功率）
  5. 长时间连续规划（验证场景不累积垃圾物体）

指标：成功率、p50/p99 规划耗时、路径长度、逆解调用次数

# moveit_ros_benchmarks 可对多个规划器做定量对比
ros2 launch moveit_ros_benchmarks panda_benchmark.launch.py \
  planner_configs:=RRTConnect BITstar
# 输出成功率、耗时分布、路径长度，用于选型决策