两段轨迹之间的 Blending 实现说明

当前流程简述

在 hivecore_robot_motion 中，臂控流程为：

规划：通过 MoveIt（PlanJointMotion / PlanCartesianMotion / PlanDualArmJointMotion）得到一条关节轨迹。
时间参数化：在 ArmControl::timeParameterizeAndStore_ 中对轨迹做 S 型曲线或梯形时间参数化，得到带 time_from_start、positions/velocities/accelerations 的 JointTrajectory。
下发：DualArmAction 将规划结果导出为 JointTrajectory，按左/右臂拆分为两条轨迹，通过 FollowJointTrajectory (FJT) action 分别发给 left_arm_controller 和 right_arm_controller。

目前每个 DualArm goal 只包含一条运动（单臂 1 个目标，双臂 2 个目标），不存在“两段轨迹”的概念，因此也没有段与段之间的 blending。

两段轨迹 Blending 的两种实现思路

思路一：在 motion 层合并两段轨迹再下发（推荐）

在应用层得到“两段”轨迹后，在 下发前 将两段轨迹在关节空间做平滑过渡（blend），合并成一条 JointTrajectory，再通过 FJT 一次性下发。这样：

不需要底层 FJT/controller 支持 blend；
与现有“单条轨迹 → FJT”的流程一致；
实现集中在 robot_control 内，易于维护。

实现位置：已提供工具函数 BlendTwoTrajectories()（见 include/utils/trajectory_blend.hpp），输入两段 JointTrajectory 和 blend_ratio，输出合并后的轨迹。

如何得到“两段”轨迹（需扩展规划/action）：

方式 A：支持“从指定关节状态规划到目标”的接口（例如 PlanJointMotionFromState(arm_id, start_joints, target_joints, ...)），内部用 move_group->setStartState(...) 设置起点后再 plan()。
- 第一段：当前状态 → 中间点（PlanJointMotion 现有逻辑）；
- 第二段：中间点 → 终点（调用 PlanJointMotionFromState）；
- 对两段结果调用 BlendTwoTrajectories(seg1, seg2, blend_ratio, blend_point_count, &merged)，再将 merged 导出并下发。
方式 B：扩展 DualArm goal，支持每个臂多个路点（如 arm_motion_params 变为多段，或新增 waypoints 字段）。对每个臂依次规划段 1、段 2、…，再按段顺序两两 blend 成一条轨迹后下发。

接口里已有 ArmMotionParams.blend_radius（0–100），可映射为 BlendRadiusToRatio(blend_radius) 作为 blend_ratio 传入 BlendTwoTrajectories。

思路二：控制器侧 Blending（若硬件/驱动支持）

若底层 FJT 或关节控制器支持“多段轨迹 + 每段 blend 半径”（类似部分工业机械臂），则可以在 不合并轨迹 的前提下，连续下发多段轨迹，由控制器在段与段之间做 blend。当前 ROS 2 常用的 joint_trajectory_controller 一般不支持该能力，需要自定义 controller 或厂商驱动支持。

已提供的工具用法

1. `BlendTwoTrajectories`

声明：include/utils/trajectory_blend.hpp
作用：将两段 JointTrajectory 在连接处按比例做关节空间平滑混合，输出一条合并轨迹。
约束：两段轨迹的 joint_names 必须一致；traj1 末点与 traj2 首点越接近，blend 越自然。
参数：
- blend_ratio：两端参与混合的比例，例如 0.2 表示两段各约 20% 参与过渡；0 表示不混合、仅时间连续拼接。
- blend_point_count：blend 区插点数，建议 5–20。

2. `BlendRadiusToRatio`

将接口层的 blend_radius（0–100）转为内部 blend_ratio（0 表示不混合）。

3. 与现有流程的衔接

规划：若支持“从指定状态规划”，则先规划段 1（当前→中间点），再规划段 2（中间点→终点）。
Blend：对左臂/右臂分别：BlendTwoTrajectories(seg1_left, seg2_left, BlendRadiusToRatio(blend_radius), 10, &left_merged)，右臂同理。
下发：与现在一致，将 left_merged / right_merged 通过 FJT 下发给左右臂 controller，无需改 FJT 或底层控制器。

小结

项目	说明
当前是否支持两段 blend	否；当前每个 goal 仅一条轨迹。
推荐实现方式	在 motion 层用 `BlendTwoTrajectories` 合并两段轨迹，再通过 FJT 单条下发。
接口预留	`ArmMotionParams.blend_radius` 已存在，可映射为 blend 比例。
规划扩展	需支持“从指定关节状态规划”（如 `PlanJointMotionFromState`）或“多路点/多段”goal，才能得到两段轨迹并做 blend。

在实现多段 goal 或“从指定状态规划”后，在导出并下发轨迹前调用 BlendTwoTrajectories 即可完成两段轨迹之间的 blending。

4.8 KiB Raw Blame History Unescape Escape