49 lines
1.3 KiB
Plaintext
49 lines
1.3 KiB
Plaintext
flowchart TD
|
|
A[NormalizedRobotModel] --> B[createRobotFromModel]
|
|
B --> C[RobotHandle]
|
|
C --> D[KDL Chain / Solvers Cache]
|
|
|
|
E[Motion Request] --> F{motion}
|
|
F -->|MOVEJ| G[planMoveJ]
|
|
F -->|MOVEL| H[planMoveL]
|
|
F -->|MOVEC| I[planMoveC]
|
|
F -->|PATH| J[planPath / validatePath]
|
|
|
|
G --> G1[校验 startJoints 和限位]
|
|
G1 --> G2{target 类型}
|
|
G2 -->|joint_target| G3[qEnd = target.joints]
|
|
G2 -->|pose_target| G4[IK 求 qEnd]
|
|
G3 --> G5[关节差分]
|
|
G4 --> G5
|
|
G5 --> G6[梯形速度曲线]
|
|
G6 --> G7[采样关节位置/速度/加速度]
|
|
G7 --> G8[FK 输出 TCP]
|
|
G8 --> R[TrajectoryResult]
|
|
|
|
H --> H1[FK 得到起点 TCP]
|
|
H1 --> H2[applyToolAndFrame / applyOffset]
|
|
H2 --> H3[直线位置和姿态插补]
|
|
H3 --> H4[梯形速度曲线]
|
|
H4 --> H5[逐点 IK]
|
|
H5 --> H6[限位/速度/奇异性检查]
|
|
H6 --> R
|
|
|
|
I --> I1[FK 得到起点 TCP]
|
|
I1 --> I2[via/target 位姿变换]
|
|
I2 --> I3[三点退化检查]
|
|
I3 --> I4[圆心/半径/法向/弧长]
|
|
I4 --> I5[圆弧采样]
|
|
I5 --> I6[逐点 IK]
|
|
I6 --> I7[圆弧误差和限位检查]
|
|
I7 --> R
|
|
|
|
J --> J1[按 segment 顺序规划]
|
|
J1 --> J2[上一段终点关节作为下一段起点]
|
|
J2 --> J3[合并轨迹点和诊断]
|
|
J3 --> J4[保留 sourceMap]
|
|
J4 --> P[PathPlanResult / PathValidationResult]
|
|
|
|
R --> D1[MotionDiagnostic]
|
|
P --> D1
|
|
D1 --> D2[error / warning / info]
|