HiveCoreRD/hivecore_robot_motion
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update doc

Browse Source
This commit is contained in:
NuoDaJia
2026-03-04 14:08:07 +08:00
parent 87a3bf68f9
commit 5d2a300c85
2 changed files with 386 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
docs/MC-S-002_点到点运动（PTP）设计方案4.md
View File

@@ -31,6 +31,26 @@
* 在PTP运动规划中，系统应确保机器人到达目标位置时无振荡，到达目标后，位置波动范围 ≤ ±0.05mm。
### 2.1 状态反馈 SMART 需求（PTP）
| 编号 | SMART 需求 |
| --- | --- |
| PTP-FB-01 | 系统必须通过 ROS 2 话题持续发布运动状态，发布频率不低于 **100Hz**（目标值）；当前实现基线为 `robot_arm_status_period_ms=10~20ms`（50~100Hz），需通过参数与性能优化收敛到 100Hz 稳态。 |
| PTP-FB-02 | 每次 PTP 任务结束（成功/失败/取消）时，必须通过 Action Result 返回 `success/final_progress/message`，并补充统一错误码语义（当前为 message 文本，后续结构化）。 |
| PTP-FB-03 | 状态信息必须具备完整性：关节空间（名称/位置/使能/错误码）+ 双臂末端笛卡尔位姿；base 与腿末端笛卡尔状态作为扩展字段进入后续版本。 |
| PTP-FB-04 | 状态消息必须携带准确时间戳并与系统时钟同步；跨 topic 对齐误差超过阈值时需上报诊断信息。 |
| PTP-FB-05 | 状态话题需支持多订阅者并发消费（建议基线 10 个）；订阅者异常断开或慢消费不得阻塞发布与运动执行链路。 |
### 2.2 状态反馈验收映射（PTP）
| 验收项 | 对应需求 |
| --- | --- |
| 实时状态：发布频率 ≥100Hz | PTP-FB-01 |
| 完成通知：Result 含成功/失败与错误信息 | PTP-FB-02 |
| 数据完整性：关节空间 + 笛卡尔空间 | PTP-FB-03 |
| 时间戳准确：与系统时钟同步 | PTP-FB-04 |
| 多订阅者支持与断开不阻塞 | PTP-FB-05 |
## 3. 依赖
@@ -145,6 +165,7 @@ flowchart LR
- `robot_arm_status_period_ms`：状态发布周期，范围 `[10, 20]` ms，默认 `20` ms。
- `velocity_scaling`：Action 目标速度比例（兼容 1~100 输入语义）。
- 状态反馈目标频率：`>=100Hz`（建议将周期参数收敛到 `10ms` 并验证 CPU 余量）。
### 5.3 主题与服务约定
@@ -286,6 +307,7 @@ geometry_msgs/Pose right_arm_pose
```
> 末端位姿采用 `jointPositions_ + FK` 计算，确保与同帧关节值一致。
> 当前版本对外稳定字段为“关节空间 + 双臂末端位姿”；base 与腿末端笛卡尔状态定义为下一版本扩展项，以保持接口兼容演进。
---
@@ -440,6 +462,7 @@ stateDiagram-v2
- 统一状态topic：`/robot_control/arm_status` 作为运维与调试主观测口。
- 建议录包：`joint_states`、`dynamic_joint_states`、`robot_control/arm_status`、Action feedback/result。
- 调试逆解：调用 `/robot_control/inverse_kinematics` 并与 FK结果做闭环比对。
- 建议新增监控项：状态发布时间抖动、Action Result 延迟、订阅者数量与发布阻塞告警。
---
@@ -481,6 +504,7 @@ stateDiagram-v2
- 连续PTP请求压力（含取消）。
- 状态topic长时间发布（10ms 周期）稳定性与CPU占用。
- 控制器重启后的恢复行为（switch_controller + service/action 重新可用）。
- 多订阅者压力（建议 10+ 并发订阅）与慢订阅者/断开场景不阻塞验证。
---

362
docs/MC-S-003-直线运动（LIN）设计方案.md Normal file
View File

@@ -0,0 +1,362 @@
# MC-S-003_直线运动（LIN）设计方案
# 双臂 LIN 运动控制设计文档（当前实现）
| 版本 | 日期 | 作者 | 描述 |
| --- | --- | --- | --- |
| v1.0.0 | 2026/03/03 | Ray | 初版，基于当前代码实现 |
## 1. 概述
机械臂直线运动（LIN）用于要求末端执行器沿笛卡尔空间直线路径到达目标位姿的场景。
当前系统在 `DualArm.action` 中通过 `ArmMotionParams.motion_type = MOVEL` 表示 LIN 请求。
当前实现目标：
- 单臂 LIN：基于 MoveIt `computeCartesianPath` 生成直线路径；
- 双臂 LIN：左右臂分别规划后做统一时间轴同步，保证同起同止；
- 兼容现有执行链路：左右臂 `FollowJointTrajectory` 下发。
---
## 2. 用户故事与 SMART 需求
### 2.1 用户故事（场景描述）
1. 上层应用需要机械臂末端沿直线路径运动到目标位置。
2. LIN 运动在笛卡尔空间规划，确保末端轨迹为直线。
3. 姿态可保持固定或做平滑插值。
4. 典型应用包括焊接、涂胶、装配等对路径精度敏感的任务。
5. 系统需支持速度比例可调，并提供参数配置与逆解等服务能力。
### 2.2 SMART 需求条目
| 编号 | SMART 需求 |
| --- | --- |
| LIN-SR-01 | 系统在接收 `MOVEL` 目标后，**50ms 内**必须完成“成功规划”或“失败返回”；失败时返回结构化错误信息（`code + message + stage`）。 |
| LIN-SR-02 | 单臂 LIN 规划必须使用笛卡尔直线路径生成机制（`computeCartesianPath`），且路径完整度 `fraction >= 0.999` 才允许进入执行阶段。 |
| LIN-SR-03 | 系统必须支持两种姿态策略：`FIXED`（姿态保持）与 `SLERP`（四元数球面插值）；每条 LIN 指令必须可选其一。 |
| LIN-SR-04 | 在 `SLERP` 模式下，姿态轨迹应连续无突变；相邻采样点不得出现翻转跳变（按四元数同向性约束处理）。 |
| LIN-SR-05 | 执行期末端实际速度不得超过设定笛卡尔速度上限；当超限持续超过设定窗口（建议 100ms）时，系统必须停止并上报。 |
| LIN-SR-06 | 系统必须支持 `velocity_scaling` 在 `1%~100%` 可调，并保证速度上限按比例缩放，缩放行为在当前任务内生效。 |
| LIN-SR-07 | 到达目标后不得出现超调振荡；在到位判定窗口内（建议 500ms）末端位置应稳定在允许误差带内。 |
| LIN-SR-08 | 若直线路径上任一点 IK 无解，规划阶段必须立即失败并返回错误（例如 `IK_NO_SOLUTION_ON_PATH`），不得下发执行轨迹。 |
| LIN-SR-09 | 若规划起始点处于奇异区，系统必须拒绝规划并返回错误（例如 `START_SINGULARITY`）。 |
| LIN-SR-10 | 若执行过程中检测到路径进入奇异区，系统必须停止当前任务并返回错误（例如 `EXEC_SINGULARITY_ABORT`）。 |
| LIN-SR-11 | 双臂 LIN 必须使用统一时间轴；左右臂开始与结束偏差应满足同步阈值（建议 ≤20ms）。 |
| LIN-SR-12 | 系统必须提供 LIN 参数配置服务与逆解服务，支持在线读取/更新关键参数（具体参数集可后续冻结）。 |
### 2.3 验收标准映射
| 验收项 | 对应需求 |
| --- | --- |
| 姿态平滑（SLERP，无突变） | LIN-SR-03, LIN-SR-04 |
| 速度控制（不超过笛卡尔上限） | LIN-SR-05, LIN-SR-06 |
| 位置精度与无超调振荡 | LIN-SR-07 |
| 规划时间 ≤ 50ms | LIN-SR-01 |
| 路径点 IK 不可达返回错误 | LIN-SR-08 |
| 起始点奇异返回错误 | LIN-SR-09 |
| 执行中经过奇异点返回错误并停止 | LIN-SR-10 |
| 速度比例可调 | LIN-SR-06 |
| 参数配置服务/逆解服务 | LIN-SR-12 |
### 2.4 状态反馈用户故事（场景描述）
1. 上层应用需要实时了解机械臂运动状态，包括当前位置、速度、运动进度等。
2. 状态反馈通过 ROS 2 话题实时发布，并支持多个订阅者同时订阅。
3. 运动完成后通过 Action Result 通知执行结果和最终状态。
4. 适用于在线监控、任务编排、异常追踪等场景。
### 2.5 状态反馈 SMART 需求条目
| 编号 | SMART 需求 |
| --- | --- |
| LIN-FB-01 | 系统必须通过 ROS 2 话题持续发布运动状态，发布频率不低于 **100Hz**；消息需包含关节空间与笛卡尔空间状态字段（手臂末端、base、腿末端），以及运动类型、运动状态与进度。 |
| LIN-FB-02 | 每次 LIN 任务结束（成功/失败/取消）时，系统必须在 Action Result 返回执行结果、错误码和最终状态摘要；Result 发送延迟应小于 **100ms**（相对任务终止时刻）。 |
| LIN-FB-03 | 系统必须支持多订阅者并发订阅状态话题（建议基线 10 个），且任一订阅者断开或消费变慢时，发布端持续发布、不阻塞控制链路。 |
### 2.6 状态反馈验收标准映射
| 验收项 | 对应需求 |
| --- | --- |
| 实时状态：发布频率 ≥100Hz | LIN-FB-01 |
| 完成通知：Result 含成功/失败状态与错误码 | LIN-FB-02 |
| 数据完整性：包含关节空间 + 笛卡尔空间字段 | LIN-FB-01 |
| 时间戳准确：与系统时钟同步 | LIN-FB-01 |
| 多订阅者支持：并发订阅不影响性能 | LIN-FB-03 |
| 订阅者断开不阻塞系统 | LIN-FB-03 |
### 2.7 PTP/LIN 状态反馈统一需求矩阵
| 维度 | PTP（MC-S-002） | LIN（本方案） | 统一结论 |
| --- | --- | --- | --- |
| 发布通道 | `/robot_control/arm_status` + Action feedback/result | `/robot_control/arm_status` + Action feedback/result | 统一 |
| 发布频率目标 | >=100Hz（目标），当前参数 10~20ms | >=100Hz（目标） | 统一为 >=100Hz，按性能分阶段收敛 |
| 数据范围 | 关节空间 + 双臂末端位姿（当前） | 关节空间 + 笛卡尔状态（当前重点手臂） | 统一扩展到“关节+手臂/base/腿末端” |
| 完成通知 | Action Result（success/message/final_progress） | Action Result（success/message/final_progress） | 统一错误码语义（后续结构化） |
| 多订阅者鲁棒性 | 设计要求支持并发与断开不阻塞 | 设计要求支持并发与断开不阻塞 | 统一压测基线 10+ 订阅者 |
| 时间戳一致性 | 需与系统时钟对齐 | 需与系统时钟对齐 | 统一校验策略 |
---
## 3. 依赖
- **ROS 2 / rclcpp**
- **MoveIt 2**：`MoveGroupInterface`、`computeCartesianPath`
- **ros2_control**：`JointTrajectoryController`
- **control_msgs/action/FollowJointTrajectory**
- **interfaces/action/DualArm**
- **interfaces/msg/ArmMotionParams**
---
## 4. 内部结构设计（按当前代码）
### 4.1 总体架构
```mermaid
flowchart LR
Client[上层应用<br/>DualArm Action Client] --> DA[DualArmAction]
DA --> RCM[RobotControlManager]
RCM --> AC[ArmControl]
AC --> MGI_L[MoveGroup arm_left]
AC --> MGI_R[MoveGroup arm_right]
DA --> TU[trajectory_utils]
DA --> LFJT[left_arm_controller FJT]
DA --> RFJT[right_arm_controller FJT]
```
### 4.2 分层职责
#### `DualArmAction`（业务编排层）
- LIN 目标校验与执行状态机；
- 单臂/双臂分支调度；
- 双臂 LIN 同步重采样调用；
- FJT 下发、反馈发布、取消和异常处理。
#### `RobotControlManager`（领域能力聚合）
- 提供 `PlanArmMotion` / 轨迹导出等统一能力；
- 将 `MOVEL` 请求路由至 `ArmControl::PlanCartesianMotion`。
#### `ArmControl`（MoveIt 适配层）
- `PlanCartesianMotion` 调用 `computeCartesianPath`；
- 轨迹时间参数化并写入缓存；
- 导出单臂轨迹供执行层使用。
#### `trajectory_utils`（轨迹工具层）
- 起点时间修正；
- 双臂时间轴同步重采样；
- S 型进度映射；
- 速度/加速度回填。
---
## 5. 配置项设计（LIN 相关）
### 5.1 同步重采样参数
| 参数名 | 默认值 | 说明 |
| --- | --- | --- |
| `dual_arm.movel_sync.use_s_curve_progress` | `true` | 是否启用 S 型进度映射 |
| `dual_arm.movel_sync.min_dt_sec` | `0.01` | 同步轨迹采样的最小时间分辨率 |
| `dual_arm.movel_sync.max_samples` | `500` | 同步轨迹最大采样点数 |
### 5.2 轨迹执行参数
- `velocity_scaling`：来自 `DualArm.goal`（1~100 映射为 [0,1]）；
- `acceleration_scaling`：当前执行链路固定使用内部配置值（与 PTP 一致）。
---
## 6. 接口定义（当前实现）
### 6.1 LIN 目标定义
```plaintext
# ArmMotionParams.msg (节选)
uint8 MOVEJ = 0
uint8 MOVEL = 1
uint8 arm_id
uint8 motion_type
geometry_msgs/Pose target_pose
float64[] target_joints
```
说明：
- `MOVEL` 仅使用 `target_pose`；
- `MOVEJ` 仅使用 `target_joints`。
### 6.2 执行入口
- 统一通过 `DualArm.action` 触发；
- 单臂：`arm_motion_params.size() == 1`；
- 双臂：`arm_motion_params.size() == 2`（需包含 left/right）。
---
## 7. LIN 行为与流程设计
### 7.1 单臂 LIN 规划流程
```mermaid
sequenceDiagram
participant C as Client
participant DA as DualArmAction
participant RCM as RobotControlManager
participant AC as ArmControl
participant FJT as Arm FJT
C->>DA: Goal(单臂, MOVEL)
DA->>RCM: PlanArmMotion(MOVEL)
RCM->>AC: PlanCartesianMotion
AC->>AC: computeCartesianPath(current->target)
AC-->>RCM: 轨迹缓存
RCM-->>DA: ExportArmPlannedTrajectory
DA->>FJT: async_send_goal(traj)
FJT-->>DA: result
DA-->>C: Action Result
```
### 7.2 双臂 LIN 规划与同步流程
```mermaid
sequenceDiagram
participant C as Client
participant DA as DualArmAction
participant RCM as RobotControlManager
participant TU as trajectory_utils
participant LFJT as Left FJT
participant RFJT as Right FJT
C->>DA: Goal(双臂, 含MOVEL)
DA->>RCM: PlanArmMotion(left)
DA->>RCM: PlanArmMotion(right)
DA->>RCM: ExportArmPlannedTrajectory(left/right)
DA->>TU: synchronize_dual_arm_trajectory_timebase
TU-->>DA: left_sync/right_sync
DA->>LFJT: send left_sync
DA->>RFJT: send right_sync
LFJT-->>DA: result
RFJT-->>DA: result
DA-->>C: Action Result
```
---
## 8. 直线性与同步性设计说明
### 8.1 单臂直线性保证
单臂 `MOVEL` 直线性由 `computeCartesianPath` 保证，关键点：
- 使用当前位姿与目标位姿构造 waypoints；
- 采用固定 `eef_step` 离散；
- 要求 `fraction` 接近 1（当前阈值为 `>= 0.999`）。
### 8.2 双臂同步策略
当前双臂 LIN 不做“联合笛卡尔约束规划”，而是：
1. 左右臂各自直线规划；
2. 统一时间轴重采样；
3. 并发下发 FJT。
该策略可在保持各臂 LIN 路径特征的同时，提高双臂执行同步性。
### 8.3 平滑性策略
同步重采样时支持：
- S 型进度映射（quintic smoothstep）；
- 重采样后回填 `vel/acc`；
- 配合非零起点时间处理，降低起步冲击。
---
## 9. 取消、急停与异常处理
### 9.1 取消
- 检测到 cancel 请求后：
- 发送减速/保持轨迹；
- 取消当前 FJT goal；
- Action 返回 canceled。
### 9.2 急停
- 执行循环内持续检查急停锁存；
- 触发后立即停止并 abort。
### 9.3 规划失败与参数非法
- 无法生成笛卡尔路径；
- 目标位姿非法（NaN/Inf/四元数异常）；
- 导出轨迹失败；
- 控制器不可用或 goal 被拒绝。
均通过 Action Result 的 `success=false` 与 `message` 返回。
---
## 10. 可观测性与调试支持
- Action Feedback：
- `status`：规划/执行/完成；
- `progress`：执行进度估算值。
- 统一状态 topic：
- `/robot_control/arm_status` 提供关节空间与笛卡尔状态观测；
- 建议与 `joint_states`、`dynamic_joint_states` 同步录包做一致性比对。
- 轨迹文件导出：
- 可保存左右臂下发轨迹（CSV）用于离线分析。
- 关键日志：
- LIN 规划成功率、同步重采样参数、FJT 接受/执行结果。
- 状态发布频率、Action Result 延迟、多订阅者阻塞告警。
---
## 11. 测试设计
### 11.1 功能测试
- 左臂/右臂单臂 LIN 到达测试；
- 双臂 LIN 同步执行测试；
- `MOVEJ + MOVEL` 混合输入测试。
### 11.2 直线性测试
- 记录末端实际轨迹并做线段拟合；
- 评估最大偏离与平均偏离。
### 11.3 同步性测试
- 比较左右臂完成时间差；
- 比较相同归一化时刻的状态一致性。
### 11.4 稳定性测试
- 长时间连续 LIN；
- 随机注入 cancel/急停；
- 重复启动/停止场景回归。
- 多订阅者并发 + 慢订阅者/断开场景下的非阻塞验证。
---
## 12. 边界与后续优化
### 12.1 当前边界
- 双臂 LIN 采用“单臂规划 + 同步重采样”，并非全局联合最优；
- 时长差异较大时，快臂会被拉伸到慢臂时长；
- 动态品质仍受控制器参数影响。
### 12.2 后续优化建议
- 引入双臂联合笛卡尔约束规划；
- 增加在线同步偏差监控指标；
- 将同步重采样参数纳入任务级可配置输入；
- 增加 LIN 段间 blending 设计。