这件事本身：输入 5 个角，输出 6 个数
已知：5 个关节角 q1 ~ q5
例：(0°, −50°, 105°, 35°, 0°)
算
要求：TCP 位姿 = 位置 3 个数 + 姿态 3 个数
位置 (0.157, 0, 0.342) m 姿态：工具段水平、夹爪朝向 q1+q5
学它的七级台阶
前四级手算打通（让你懂），第 5、6 级换成矩阵（让程序能算），两条都得走
第一步 · 手算打通：用三角函数一段段算出来
第二步 · 工程实现：用矩阵重写，同样必学
第 1 级
一根棍子
前 L·sinθ，上 L·cosθ
工具：sin / cos
第 2 级
朝哪边伸
航向角投到 x-y 平面
工具：水平面分解
第 3 级
三节串起来
角度累加 q2、q2+q3…
工具：向量相加
第 4 级 · ★ 走到这里，你就真的懂了
手算出位姿：位置 + 姿态
位置 (0.157, 0, 0.342) m 姿态：累计角 −50+105+35 = 90°，工具段水平
一个计算器就算得完，每一步都知道自己在干什么
第 5 级
旋转矩阵 2×2 / 3×3
三段公式打包成一个模板，套角度就出结果
3×3 的三列 = 物体三根轴的方向
新工具：旋转矩阵
第 6 级
齐次矩阵 4×4
「转 + 挪」合成一次乘法，可以一直连乘
ROS / URDF 内部就是这么算的
程序里跑的就是这一套，新工具：齐次矩阵
同答案
多能力
第 7 级
上真机：PWM → 关节角 → 位姿，再用卷尺校对
rad = (PWM − 1500) × 0.002356，套同一套公式；pwm_to_tcp.py 直接打印位置和姿态
验收：手算、程序输出、卷尺测量，三者差在 5 mm 以内
第 1~3 级：打通手算
第 4 级：目标达成
第 5~6 级：矩阵重建，必学
第 7 级：真机核对
先学这个，再学那个
同一答案，多一种能力