末端位姿
配套页面:控制台上的「末端位姿」页签。
1. 位姿 = 位置 + 姿态#
位置说的是夹爪指尖那个点(TCP)在空间里的坐标;姿态说的是夹爪以什么角度朝过去。两者合起来才叫一个"位姿"。
它和关节角是两个层面的东西:
| 描述的是 | 谁在管 | |
|---|---|---|
| 六个关节角 | 手臂怎么摆的 | 舵机,是唯一真相 |
| 末端位姿 | 末端在哪、朝哪 | 从关节角正解算出来的 |
关节角是底层状态,位姿是上层表达。你给一个位姿,SDK 用逆解把它翻译成关节角;反过来,关节角经过正解就得到位姿。这也解释了为什么"位姿对了"不代表"姿态唯一"—— 同一个位姿可能对应不同的手臂构型(肘上 / 肘下)。
2. 五个分量#
Pose 就是这五个数,单位是 mm 和度:
| 分量 | 是什么 | 说明 |
|---|---|---|
x |
左右位置 | 基座系,向左为正 |
y |
前后位置 | 向后为正;手臂伸出时是负值 |
z |
高度 | 向上为正;桌面在 30.1 mm,不是 0 |
pitch |
进近角 | 夹爪指向与水平面的夹角,90 = 竖直向下 |
roll |
绕进近轴的转角 | 直接等于 wrist_roll 关节角 |
前三个定"在哪",后两个定"怎么朝向"。
3. 自由度:为什么只有 5 个数#
要把一个刚体在空间里完全定死,需要 6 个自由度:3 个位置 + 3 个姿态(roll / pitch / yaw)。这台臂的末端只有 5 个——少的那一个是 yaw。
| 分量 | 能不能控 | 为什么 |
|---|---|---|
| x / y / z | 能 | 由底座旋转、大臂、肘部共同决定 |
| pitch | 能 | 由 wrist_flex 给 |
| roll | 能 | 由 wrist_roll 直接给 |
| yaw | 不能 | 手臂指向某个点后,水平朝向就唯一确定了 |
换句话说:位置 3 个自由度 + 姿态 2 个自由度 = 5。这也是为什么 Pose 只有五个字段,而不是六个。
少一个 yaw 意味着什么#
手臂指向某个点之后,进近方向在水平面上的投影就定死了"从哪个方位靠近"。你没法在保持位置的同时,让夹爪换个水平朝向。
实际后果:
- 想让夹爪换个水平朝向,只能改
x/y—— 位置跟着变 pitch=90(竖直向下)时,进近方向的水平投影退化成零,yaw 连"是什么"都谈不上- 有些位姿"看着能抓",实际是 5 自由度到不了的角度
求解器是怎么处理的#
逆解只解 4 个未知数:shoulder_pan / shoulder_lift / elbow_flex / wrist_flex,对 4 个残差(x, y, z, 姿态)。wrist_roll 不由求解器决定,直接取目标 roll。
这 4 个未知数怎么迭代、两个求解器("lm" 与 "riemann")差在哪儿、为什么
pitch = 90 是个敏感位置,都在 第 4 章:FK 与 IK 的第 4 节。
4. 这些数字相对哪儿:基座系#
界面里的 x / y / z 是 基座坐标系——URDF 里 base 这个 link 的坐标。原点是 base link 的原点,它在桌面下方 30.1 mm,不在桌面上。
| 轴 | 基座系指向 |
|---|---|
| +X | 向左 |
| +Y | 向后(所以手臂伸出时 y 是负的) |
| +Z | 向上 |
零位时手臂沿 −Y 伸出去,TCP 落在 (20.4, −381.1, 256.8) mm。
三维视图里画了这组轴:白点是原点,红色 +X(左)、绿色 +Y(后)、蓝色 +Z(上)。转动视角时它们跟着转,可以随时确认"现在看到的这个方向对应哪个轴"。
桌面不在 z = 0。URDF 的 base 原点比桌面低 30.1 mm(
table_z_mm默认值),所以桌面是z ≈ 30.1 mm。界面上那行"地面 = 桌面(基座系 z = 30.1 mm)"就是提醒这个。换句话说:z填 0 意味着把末端放到桌面以下 30 mm,安全层会拦。
怎么确认桌面高度#
桌面高度不是永远 30.1 mm。不同桌子、垫板、底座安装高度都会变;z 是基座系,所以最可靠的办法是用真机 TCP 去“碰”一下桌面,而不是照抄文档里的数字。
界面路径:动作区「示教」→ 遥测区「桌面高度」→「用当前末端」→「保存」。
- 连接真机,确认工作区清空、急停可触达,用手扶住机械臂。
- 点动作区的 「示教」(顶部出现「示教模式」横幅,力矩释放);扶着 TCP 慢慢下降,直到 TCP 刚好接触桌面。
- 保持 TCP 不动,在「遥测」区找到 「桌面高度」 写入框,点右侧 「用当前末端」(也就是“用当前末端填入”)——它会把当前 TCP 的
z填进去。 - 点 「保存」;提示保存成功后,右侧「禁入线 z ≥ …」会按新桌面更新。
- 点 「完成示教」 按钮上力;之后坐标解释和安全层都以这张桌子的高度为准。
注意:
- 是 TCP 碰桌面,不是夹爪外壳;下降要慢,接触即可,不要压桌面。
- 示教时没有抱闸,必须一直扶住手臂;不要在手离开时保持示教状态。
- 换桌子、加垫板、改底座安装高度,都要重新确认并保存;不要沿用上一次的值。
- 这一步只是告诉安全层“桌面在哪”,不是关闭安全区;急停、过温、跟踪误差仍然生效。
TCP 是哪个点#
界面上的 X/Y/Z 是 TCP——gripper 连杆上的固定指尖接触点(不是两指中点),也不是夹爪的转动中心:
| 基座系坐标 (mm) | |
|---|---|
| 夹爪转动中心 | (20.6, −277.5, 266.9) |
| TCP | (20.4, −381.1, 256.8) |
| 相距 | 104.1 mm |
换夹爪或加装工具,这个偏移就要重新标定:
[kinematics]
tcp_offset_mm = [-10.1, -0.2, -103.6]
pitch 和 roll 量的是什么#
| 分量 | 定义 | 实测 |
|---|---|---|
pitch |
进近方向与水平面的夹角,90 = 竖直向下 | wrist_flex=45° → pitch=45,进近 (0, −0.71, −0.71) |
roll |
绕进近轴的转角,直接等于 wrist_roll 关节角 |
wrist_roll=90° → roll=90,进近方向不变 |
pitch=90 时进近方向是 (0, 0, −1),此时手臂最高只到桌面上方约 80 mm。
一个容易忽略的细节:转动
wrist_roll会让 TCP 动一点点——因为 TCP 相对腕部轴线有约 10 mm 的横向偏移(tcp_offset的 x 分量是 −10.1 mm)。所以"腕滚不影响末端位置"只是近似。
脚本里也一样#
starm 这套 SDK 对外就是基座系,和界面完全一致:
with Arm.sim() as arm:
arm.move_pose(y=-200, z=90, pitch=60) # 基座系 mm
print(arm.get_pose()) # Pose(x=…, y=…, z=…, pitch=…, roll=…)
不需要记第二套坐标。唯一要记住的还是那句:桌面在 z ≈ 30.1 mm,不是 0。
5. 怎么读一组数#
界面上出现 (0, −260, 150, 65, 0) 时,逐项念:
| 分量 | 值 | 读作 |
|---|---|---|
| x | 0 | 在底座正中间,不偏左也不偏右 |
| y | −260 | 在底座前方 260 mm |
| z | 150 | 离底座原点 150 mm,也就是离桌面约 120 mm |
| pitch | 65 | 夹爪斜向下 65°(90° 才是竖直向下) |
| roll | 0 | 夹爪绕自身轴没转 |
6. 两种路径:joint 和 line#
填好五个数、点「移动」之前,还有一个选项决定"怎么走过去"。它不改变终点,只改变中间的过程。
joint |
line |
|
|---|---|---|
| 做法 | 只在起点算一次逆解,然后六个关节角各自插值 | TCP 沿空间直线走,每个控制周期(30 Hz)重算一次逆解 |
| TCP 轨迹 | 弧线 | 直线 |
| 速度 | 快 | 慢 15–20% |
实测(从零位出发,只改路径):
| 目标 | 弦长 | joint 偏离直线 |
joint 时长 |
line 偏离直线 |
line 时长 |
|---|---|---|---|---|---|
| 前方低位 | 163 mm | 26.2 mm | 5.00 s | 0.0 mm | 5.85 s |
| 左侧 | 247 mm | 41.1 mm | 4.51 s | 0.0 mm | 5.44 s |
joint 为什么会画弧:它插值的是关节角,而末端位置是这些角度的非线性函数。六个角各自平滑变化,末端自然走曲线。行程越长、越靠近工作空间边缘,弧得越厉害。
joint#
- 优点:快,规划简单;大范围转移、回零最合适
- 风险:弧线会扫过中间的空间。贴着桌面横移时,弧可能扎进桌面
line#
- 优点:TCP 轨迹可预测,适合下探抓取、贴面平移、避障
- 代价:每个周期都要解一次逆解(更吃 CPU),而且更慢——它要按路径上最陡的那一段关节速率配速,否则某个关节会顶到速度上限,把 TCP 拽离直线
- 额外约束:求解时禁止"换构型"(
allow_reseed=False)。否则中途换一个肘上 / 肘下解,手臂会突然横扫过去
一个内置的安全网#
line 在动作开始之前会沿直线采样 32 个点逐点试解。任何一点无解,就在动之前报错:
Unreachable: straight-line path leaves the workspace 43% of the way along it: ...
怎么选#
| 场景 | 用哪个 |
|---|---|
| 从 A 点大范围挪到 B 点、回零 | joint |
| 垂直下探去抓 / 放下 | line |
| 贴着桌面平移 | line |
只改 roll(夹爪自转) |
都行 |
7. 在页面上动手#
打开「末端位姿」页签:
- 点「取当前」 —— 把输入框填成机械臂此刻的位姿
- 改一个分量,点「IK 预检」 —— 只算不动,看它够不够得着
- 点「移动」 —— 真的走过去
- 切
joint/line路径再走一次 —— 同样的起点终点,joint让 TCP 走弧线、line走直线,轨迹完全不同。看第 6 节了解两者的取舍 - 把速度调下来再走一次 —— 速度滑块在「末端位姿」和「关节控制」两处都有,改一处另一处跟着变。它是"安全层允许的最大关节速度(默认 60 °/s)"的比例:0.4 就是 24 °/s,0.1 只剩 6 °/s。真机上先按 0.15 试,看着不慌了再加
两个值得做的实验:
- 把
z填 20(比桌面 30.1 还低)→ 安全层直接拒绝,SafetyError - 把
z填 230、pitch填 90,点「IK 预检」→Unreachable,因为竖直向下时最高只到桌面上方 80 mm 左右
8. 常见误区#
| 说法 | 对不对 | 说明 |
|---|---|---|
| "位姿就是关节角" | ✗ | 关节角是底层状态,位姿是它正解出来的结果 |
"z = 0 是桌面" |
✗ | 桌面在 30.1 mm;z=0 在桌面以下 |
| "5 个数字就是 5 自由度,所以和 6 轴臂只差一点" | ✗ | 差的 yaw 恰恰是"任意朝向"的关键 |
| "转腕滚不影响末端位置" | ≈ | TCP 有约 10 mm 横向偏移,会画小圆 |
| "移动到了就是夹爪到了" | ✗ | 到的是 TCP(固定指尖接触点),比夹爪转动中心还往前 104 mm |
参考#
- 第 2 章:机械臂关节 —— 五个关节分别管什么
- 代码里:
starm/types.py的Pose、starm/kinematics.py的pose()/ik()