机器人 / 系统课时

从 URDF 关节树提取位姿链

从 URDF 关节树提取位姿链

给定一份 URDF 和一组关节位置,目标是找出从基座到指定连杆的关节路径,再按顺序组合每段静态安装变换和关节运动变换,得到末端相对基座的完整位姿。本例包含旋转、固定、移动三类关节,并用 Python 标准库验证计算结果。

学习目标

  • 从 link、joint、parent 和 child 构建关节树,提取指定祖先路径。
  • 根据 origin、axis 和关节位置正确累乘齐次变换。
  • 对照位置、方向与关节单位核验结果,识别常见偏差。

从树中找出目标路径

URDF 中的 link 是连杆坐标系节点,joint 通过 parent 和 child 连接两个节点。对于有效的树结构,每个非根连杆只有一个父关节。可以建立 child → (parent, joint) 索引,从 tool_link 向上回溯到 base_link,再反转成从基座到末端的顺序。

URDF 关节树和末端目标路径

这里要求 base_link 是 tool_link 的祖先。若指定两条不同分支中的连杆,必须先找公共祖先,并对逆向经过的变换求逆;不能直接用本例的单向累乘方法。

固定安装变换与关节运动的顺序

每个 joint 的 origin 给出父连杆坐标系到关节零位坐标系的静态变换。xyz 单位为米;rpy 单位为弧度,按固定轴先 X 滚转、再 Y 俯仰、最后 Z 偏航解释,因此旋转矩阵按 Rz(yaw) · Ry(pitch) · Rx(roll) 组合。

同一条父子关系的变换为:

Tpc(q)=ToriginTmotion(q)T_{pc}(q)=T_{origin}T_{motion}(q)

axis 定义在关节坐标系内,省略时默认沿局部正 X 轴。revolute 与 continuous 沿该轴旋转,关节值使用弧度;prismatic 沿该轴平移,关节值使用米;fixed 的运动变换是单位矩阵。计算时先应用 origin,再应用关节运动,不能交换乘法顺序。

准备包含旁支的 URDF

把下面内容保存为与计算脚本同目录的 robot.urdf。肩关节安装高度为 0.4 米,腕部固定偏移 1 米,滑动关节安装偏移 0.5 米;另有传感器旁支,便于检查路径筛选是否正确。

xml
<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 轴。

python
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):

text
链路: 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 是否遗漏;若多算出传感器高度,说明没有排除无关旁支。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

从 URDF 关节树提取位姿链

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通