机器人 / 系统课时
从 URDF 关节树提取位姿链
从 URDF 关节树提取位姿链
给定一份 URDF 和一组关节位置,目标是找出从基座到指定连杆的关节路径,再按顺序组合每段静态安装变换和关节运动变换,得到末端相对基座的完整位姿。本例包含旋转、固定、移动三类关节,并用 Python 标准库验证计算结果。
学习目标
- 从
link、joint、parent和child构建关节树,提取指定祖先路径。 - 根据
origin、axis和关节位置正确累乘齐次变换。 - 对照位置、方向与关节单位核验结果,识别常见偏差。
从树中找出目标路径
URDF 中的 link 是连杆坐标系节点,joint 通过 parent 和 child 连接两个节点。对于有效的树结构,每个非根连杆只有一个父关节。可以建立 child → (parent, joint) 索引,从 tool_link 向上回溯到 base_link,再反转成从基座到末端的顺序。

这里要求 base_link 是 tool_link 的祖先。若指定两条不同分支中的连杆,必须先找公共祖先,并对逆向经过的变换求逆;不能直接用本例的单向累乘方法。
固定安装变换与关节运动的顺序
每个 joint 的 origin 给出父连杆坐标系到关节零位坐标系的静态变换。xyz 单位为米;rpy 单位为弧度,按固定轴先 X 滚转、再 Y 俯仰、最后 Z 偏航解释,因此旋转矩阵按 Rz(yaw) · Ry(pitch) · Rx(roll) 组合。
同一条父子关系的变换为:
axis 定义在关节坐标系内,省略时默认沿局部正 X 轴。revolute 与 continuous 沿该轴旋转,关节值使用弧度;prismatic 沿该轴平移,关节值使用米;fixed 的运动变换是单位矩阵。计算时先应用 origin,再应用关节运动,不能交换乘法顺序。
准备包含旁支的 URDF
把下面内容保存为与计算脚本同目录的 robot.urdf。肩关节安装高度为 0.4 米,腕部固定偏移 1 米,滑动关节安装偏移 0.5 米;另有传感器旁支,便于检查路径筛选是否正确。
<robot name="demo">
<link name="base_link"/>
<link name="arm_link"/>
<link name="wrist_link"/>
<link name="tool_link"/>
<link name="sensor_link"/>
<joint name="shoulder" type="revolute">
<parent link="base_link"/>
<child link="arm_link"/>
<origin xyz="0 0 0.4" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="10" velocity="1"/>
</joint>
<joint name="wrist_mount" type="fixed">
<parent link="arm_link"/>
<child link="wrist_link"/>
<origin xyz="1 0 0" rpy="0 0 0"/>
</joint>
<joint name="extension" type="prismatic">
<parent link="wrist_link"/>
<child link="tool_link"/>
<origin xyz="0.5 0 0" rpy="0 0 0"/>
<axis xyz="1 0 0"/>
<limit lower="0" upper="0.3" effort="10" velocity="0.3"/>
</joint>
<joint name="sensor_mount" type="fixed">
<parent link="base_link"/>
<child link="sensor_link"/>
<origin xyz="0 0 0.6"/>
</joint>
</robot>这个文件中的 link 名称用于确定拓扑,joint 名称用于查找位置值。示例取 shoulder = 90°、extension = 0.2 m,固定关节不需要提供位置。
解析关节并按路径累乘
把以下脚本保存为 fk.py,使用 Python 3.10 或更新版本运行,不需要安装第三方包。它以目标连杆为起点回溯,检测不存在的连杆、重复父关节与断开的路径,再对每个关节计算 origin × motion。任意方向的旋转轴通过罗德里格斯公式处理,避免把 axis 错当成只能沿 Z 轴。
import math
import xml.etree.ElementTree as ET
def identity():
return [[float(i == j) for j in range(4)] for i in range(4)]
def mul(a, b):
return [[sum(a[i][k] * b[k][j] for k in range(4))
for j in range(4)] for i in range(4)]
def triple(elem, attr, default):
text = elem.get(attr, default) if elem is not None else default
values = tuple(map(float, text.split()))
if len(values) != 3:
raise ValueError(f"{attr} 必须有三个分量")
return values
def rotation(axis, angle):
x, y, z = axis
length = math.sqrt(x*x + y*y + z*z)
if length == 0:
raise ValueError("关节轴不能为零向量")
x, y, z = x/length, y/length, z/length
c, s, v = math.cos(angle), math.sin(angle), 1 - math.cos(angle)
t = identity()
t[0][:3] = [c+x*x*v, x*y*v-z*s, x*z*v+y*s]
t[1][:3] = [y*x*v+z*s, c+y*y*v, y*z*v-x*s]
t[2][:3] = [z*x*v-y*s, z*y*v+x*s, c+z*z*v]
return t
def origin_pose(elem):
xyz = triple(elem, "xyz", "0 0 0")
roll, pitch, yaw = triple(elem, "rpy", "0 0 0")
# URDF 固定轴 RPY 的旋转矩阵为 Rz @ Ry @ Rx。
t = mul(mul(rotation((0, 0, 1), yaw),
rotation((0, 1, 0), pitch)),
rotation((1, 0, 0), roll))
for i in range(3):
t[i][3] = xyz[i]
return t
def joint_motion(joint, positions):
kind = joint.get("type")
if kind == "fixed":
return identity()
if kind not in ("revolute", "continuous", "prismatic"):
raise ValueError(f"暂不支持关节类型:{kind}")
name = joint.get("name")
if name not in positions:
raise ValueError(f"缺少关节位置:{name}")
axis = triple(joint.find("axis"), "xyz", "1 0 0")
q = positions[name]
if kind in ("revolute", "continuous"):
return rotation(axis, q)
length = math.sqrt(sum(v*v for v in axis))
if length == 0:
raise ValueError("关节轴不能为零向量")
t = identity()
for i in range(3):
t[i][3] = q * axis[i] / length
return t
def forward_kinematics(root, base, target, positions):
links = {item.get("name") for item in root.findall("link")}
if base not in links or target not in links:
raise ValueError("基座或目标 link 不存在")
parent_of = {}
for joint in root.findall("joint"):
parent = joint.find("parent").get("link")
child = joint.find("child").get("link")
if parent not in links or child not in links or child in parent_of:
raise ValueError(f"关节树不合法:{joint.get('name')}")
parent_of[child] = (parent, joint)
# 从目标回溯父关节,反转后才按基座到末端顺序累乘。
path, seen, current = [], set(), target
while current != base:
if current in seen or current not in parent_of:
raise ValueError("找不到从基座到目标的祖先路径")
seen.add(current)
parent, joint = parent_of[current]
path.append((current, joint))
current = parent
path.reverse()
t = identity()
for _, joint in path:
t = mul(t, mul(origin_pose(joint.find("origin")),
joint_motion(joint, positions)))
return [base] + [child for child, _ in path], t
robot = ET.parse("robot.urdf").getroot()
joint_positions = {"shoulder": math.pi / 2, "extension": 0.2}
path, pose = forward_kinematics(robot, "base_link", "tool_link",
joint_positions)
print("链路:", " -> ".join(path))
print("位姿矩阵 T_base_tool:")
for row in pose:
print(" ".join(f"{value:.3f}" for value in row))预期输出(在两份文件所在目录执行 python fk.py):
链路: base_link -> arm_link -> wrist_link -> tool_link
位姿矩阵 T_base_tool:
0.000 -1.000 0.000 0.000
1.000 0.000 0.000 1.700
0.000 0.000 1.000 0.400
0.000 0.000 0.000 1.000用数值检查坐标系和单位
输出右上角的平移列是末端原点在 base_link 中的坐标 (0, 1.7, 0.4) 米。伸出方向上的三段长度为 1 + 0.5 + 0.2 = 1.7 米,肩部绕正 Z 轴转 90° 后,原本沿 X 方向的长度全部转到正 Y;左上角旋转矩阵也对应绕 Z 轴正向旋转 90°。
把 joint_positions 改为 {"shoulder": 0.0, "extension": 0.0} 再运行,末端应回到 (1.5, 0, 0.4) 米、旋转矩阵为单位矩阵。若 Y 坐标反号,先检查正轴方向和弧度;若长度少了 0.5 米,检查 extension 的 origin 是否遗漏;若多算出传感器高度,说明没有排除无关旁支。