orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

P5c-2：仿真世界 + 机器人描述 + 控制器配置，并解决两处官方描述与 Gazebo 的不兼容

Browse Source 
这一步解决的是"官方 Flexiv 描述能不能喂给 Gazebo"的问题。结论是**不能直接用**，
有两处不兼容，且都只有 Gazebo 会暴露 —— RViz/MoveIt 完全正常，所以此前从未发现。

问题一：7 轴本体的质量/惯量是"孤儿标签"
    rizon_arm.xacro 调 get_inertias 时没写 <inertial> 包裹，而那个宏只吐
    <origin>/<mass>/<inertia> 三个碎片。RViz/MoveIt 忽略不认识的子元素所以看着
    没事；sdformat 判定 "link has no <inertial> block"，然后把 **link 和它的父
    关节一起丢掉**。症状极具误导性：机械臂凭空消失或只剩夹爪，报错只有一句
    "link[sim_link1] is not modeled in sdf"。

问题二：夹爪有零质量 link，导致 sdformat 惯性叠加出 NaN
    grav_macro.xacro 给六个纯坐标系 dummy link 写了 mass=0、惯量全 0。
    sdformat 对 fixed 关节会把子 link 惯性叠加到父 link，叠加要除以总质量 →
    质量 0 就出 NaN。而报错指向的是**父** link：
        Pose values must be finite. link[sim_left_finger_mount]/inertial/pose
    完全看不出根因在子 link 的质量上。

修法：启动时归一化（不走改上游文件的路，因为上游由 fetch_flexiv.sh 获取、改了会被
覆盖；而且换别的臂时大概率有别的坑，在启动时统一过一道更可持续）
    - 孤儿三件套 → 只重新分组已有元素，一个数字都不改
    - 零质量 → 补 1e-4 kg（与夹爪其他配件同量级，0.1g 对动力学无影响），
               惯量全 0 时补极小对角值避免张量退化
    不发明物理数据，也不掩盖真问题：上游真没给质量时这里什么都不会包。
    16 项单测，含"已有 <inertial> 不能被二次包裹"（早期用全局正则踩过，
    sdformat 报 "Inertial element must have a mass element"）与幂等性。

新增/改动
- devices/sim/gazebo/urdf/：arm.sim.urdf.xacro（主）、.ros2_control（gz 硬件）、
  .gazebo（系统插件/材质/接触刚度）、.sensors（D405 + 双指触觉）
- devices/sim/gazebo/worlds/assembly.world：1ms 步长 + 150 次接触迭代（力信号
  要干净才谈得上"切向力突变"判据）、工作台、world 级渲染插件、D435
- devices/sim/gazebo/config/controllers.yaml：控制器名 rizon_arm_controller
  **故意与真机一致** —— 隔离的是关节/TF 层（sim_ 前缀），不是应用层依赖的接口
- src/arm_sim_gazebo/：新包，含 urdf_normalize（库 + CLI）与单测

一处设计纠正：D435 从 URDF 挪进 world。原本挂在 URDF 的 world link 上，结果
整台相机转 SDF 时凭空消失（sdformat 丢弃无质量根 link 时连带丢了整棵子树，
且退出码 0、不报错）。眼在手外的相机物理上就属于场景，放 world 才对；
渲染插件也一并归 world 级，避免"插件在模型里、相机在世界里"的错配。

验证（不是只看退出码）
- 带夹爪全量：sdformat 退出码 0、0 报错、15 link / 15 joint / 26 frame
- 传感器 d405_rgbd + tactile_left/right 均在；控制器插件与参数路径正确
- 世界文件 sdformat 通过
- 包在 ros_gz_sim 环境构建通过，CLI 实跑结果与库调用一致
- 未注册 gazebo_sim_launcher 入口点：模块还没写，注册了构建能过但一运行就
  ImportError，不如老实缺着（manifest 里如实标注为未实现）

注：实测确认 gz_ros2_control 的硬件/接口声明**不在 SDF 里**，插件是从
/robot_description 话题读 URDF 的 <ros2_control> —— 所以启动器必须先起
robot_state_publisher 且发布归一化后的同一份 URDF，否则控制器注册的关节名
会和 Gazebo 里的对不上。这条已写进 P5c-3 的实现要求。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-09-23 14:19:39 +08:00
parent d93dab31c9
commit 25635e21f8
15 changed files with 1506 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/arm_sim_gazebo/arm_sim_gazebo/__init__.py
+12
View File
@@ -0,0 +1,12 @@
"""Gazebo 仿真后端。
只暴露纯函数级别的能力，不在这里 import rclpy —— 这样
urdf_normalize 可以被单测直接导入，不需要 ROS 运行时。
"""
from .urdf_normalize import ( # noqa: F401
NormalizeReport,
normalize_file,
normalize_urdf,
)
__all__ = ["NormalizeReport", "normalize_file", "normalize_urdf"]
src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py
+250
View File
@@ -0,0 +1,250 @@
"""把厂商给的 URDF 归一化成"Gazebo 能接受"的形态。
=============================================================================
为什么需要这一步（全是实测踩出来的）
=============================================================================
官方 flexiv_description 是给 RViz / MoveIt 用的，直接喂 Gazebo 会被
sdformat 拒掉。两类问题，都属于"RViz 容忍、Gazebo 严格"的差异，
所以这个框架在有 Gazebo 之前一直没暴露：
问题一：7 轴本体的质量/惯量是"孤儿标签"
rizon_arm.xacro 里这样调：
<link name="${prefix}base_link">
<xacro:get_inertias name="base_link" />
get_inertias 宏只吐 <origin> / <mass> / <inertia> 三个碎片，**外面没有
<inertial> 包裹**。URDF 规范里 <link> 不能直接有这三个子元素。
RViz/MoveIt 忽略不认识的子元素 → 看起来一切正常
sdformat 判定 "link has no <inertial> block"，
然后把 **link 和它的父关节一起丢掉**
症状很误导：机械臂凭空消失或只剩夹爪，报错只有一句
"link[sim_link1] is not modeled in sdf"，看不出根因在上游描述文件里。
问题二：夹爪有零质量 link，导致 sdformat 惯性叠加出 NaN
grav_macro.xacro 给 left_finger_tip / left_finger_tcp / grav_tcp 等
六个纯坐标系 dummy link 写了 mass=0、惯量全 0。
sdformat 对 fixed 关节会把子 link 的惯性"叠加(lump)"到父 link，
这个叠加要除以总质量 → 质量 0 就出 NaN。而报错指向的是**父** link：
Pose values must be finite.
link[sim_left_finger_mount]/inertial/pose
完全看不出是子 link 质量的问题。
=============================================================================
修法原则
=============================================================================
只做"重新分组 + 补极小量"，不发明任何物理数据，也不掩盖真问题：
- 孤儿三件套 → 只是把已有元素包进 <inertial>，一个数字都不改
- 零质量 → 补成 1e-4 kg（与夹爪其他配件同量级，0.1g 对动力学无影响），
惯量如果是全 0 也补成极小的对角值，避免张量退化
若某个 link 上游真没给质量信息，问题一这里什么都不会包，
sdformat 会照实报错 —— 这是我们想要的：归一化不是止错器。
=============================================================================
为什么在启动时做，而不是改上游文件
=============================================================================
1. 上游文件由 scripts/fetch_flexiv.sh 从 GitHub 获取，改了会被下次 fetch 覆盖
2. 这个框架要支持"换成别的臂" —— 别的厂商描述大概率有别的坑，
在启动时统一过一道"归一化"，比逐个去改厂商文件更可持续
3. 纯字符串→字符串的变换，可以脱开 ROS 单测（见 test/test_urdf_normalize.py）
=============================================================================
"""
from __future__ import annotations
import re
from dataclasses import dataclass, field
from typing import List, Tuple
# 零质量 link 补成的质量。取 1e-4 kg：与夹爪上 other 配件（内外杆、指座）
# 同量级，0.1g 在动力学上完全可忽略，但足以让惯性叠加不除零。
ZERO_MASS_FIX_KG = "1e-4"
# 惯量全 0 时补的对角值。极致小值只为让张量非退化，不引入可观测的转动惯量。
ZERO_INERTIA_FIX = "1e-9"
# 裸露的 origin + mass + inertia 三件套（自闭合标签，连续三行）。
# 夹爪的 <inertial> 块内部也是这三行，所以必须按 link 块处理并跳过
# 已含 <inertial> 的块，否则会二次包裹 —— sdformat 会报
# "Inertial element must have a mass element"（已踩过）。
_TRIPLE = re.compile(
r"(?P<ind>[ \t]*)<origin(?P<o>[^>]*?)/>[ \t]*\n"
r"[ \t]*<mass(?P<m>[^>]*?)/>[ \t]*\n"
r"[ \t]*<inertia(?P<i>[^>]*?)/>"
)
_LINK_BLOCK = re.compile(r"<link\b[^>]*>.*?</link>", re.S)
_MASS_TAG = re.compile(r"<mass(?P<attrs>[^>]*?)/>")
_INERTIA_TAG = re.compile(r"<inertia(?P<attrs>[^>]*?)/>")
_INERTIA_COMPONENTS = ("ixx", "ixy", "ixz", "iyy", "iyz", "izz")
@dataclass
class NormalizeReport:
"""归一化做了什么。启动器会打印它 —— 静默修改用户描述是不可接受的，
必须让人一眼看到"我改了你给的文件里的哪几处、改成什么"。"""
wrapped_inertials: int = 0
fixed_zero_mass: List[str] = field(default_factory=list)
fixed_zero_inertia: List[str] = field(default_factory=list)
@property
def changed(self) -> bool:
return bool(
self.wrapped_inertials or self.fixed_zero_mass or self.fixed_zero_inertia
)
def summary(self) -> str:
if not self.changed:
return "URDF 无需归一化"
bits = []
if self.wrapped_inertials:
bits.append(f"补 <inertial> 包裹 {self.wrapped_inertials} 处")
if self.fixed_zero_mass:
bits.append(
f"零质量 link 补 {ZERO_MASS_FIX_KG}kg {len(self.fixed_zero_mass)} 个"
f"({', '.join(self.fixed_zero_mass)})"
)
if self.fixed_zero_inertia:
bits.append(f"零惯量张量补极小对角 {len(self.fixed_zero_inertia)} 个")
return "；".join(bits)
def _num(attr: str) -> float:
"""把 URDF 里的数值属性转 float，失败返回 nan（调用方据此跳过）。"""
try:
return float(attr)
except (TypeError, ValueError):
return float("nan")
def _attrs_of(tag: str) -> dict:
"""从标签文本里取出 name="value" 形式的属性。"""
return dict(re.findall(r'([A-Za-z_][\w.-]*)\s*=\s*"([^"]*)"', tag))
def _fix_zero_mass(block: str, report: NormalizeReport, link_name: str) -> str:
"""把 <mass value="0"/> 补成极小正值。
两个前提：这个 link 有 <inertial>（否则不是"质量为零"而是"没有质量信息"，
不归这里管），且质量为数值 0。
"""
if "<inertial>" not in block:
return block
m = _MASS_TAG.search(block)
if m is None:
return block
attrs = _attrs_of(m.group("attrs"))
if _num(attrs.get("value", "")) != 0.0:
return block
report.fixed_zero_mass.append(link_name)
fixed = f'<mass value="{ZERO_MASS_FIX_KG}"/>'
block = block[: m.start()] + fixed + block[m.end():]
# 惯量张量若也全是 0，补极小对角值，避免退化
im = _INERTIA_TAG.search(block)
if im is not None:
iattrs = _attrs_of(im.group("attrs"))
if all(_num(iattrs.get(c, "")) == 0.0 for c in _INERTIA_COMPONENTS):
report.fixed_zero_inertia.append(link_name)
new_attrs = " ".join(
f'{c}="{ZERO_INERTIA_FIX if c in ("ixx", "iyy", "izz") else "0"}"'
for c in _INERTIA_COMPONENTS
)
block = block[: im.start()] + f"<inertia {new_attrs}/>" + block[im.end():]
return block
def normalize_urdf(urdf: str) -> Tuple[str, NormalizeReport]:
"""归一化入口。返回 (新 URDF 文本, 报告)。
纯文本替换，不改动输入里其它任何字节 —— 重写整份 XML（如用
ElementTree 再序列化）会顺带改变空白、属性顺序甚至语义，风险不值得担。
"""
report = NormalizeReport()
parts: List[str] = []
pos = 0
for m in _LINK_BLOCK.finditer(urdf):
parts.append(urdf[pos:m.start()])
block = m.group(0)
link_name = "?"
# --- 问题一：孤儿 inertial 三件套 ---
if "<inertial>" not in block:
def repl(mm: re.Match) -> str:
ind = mm.group("ind")
return (
f"{ind}<inertial>\n"
f"{ind} <origin{mm.group('o')}/>\n"
f"{ind} <mass{mm.group('m')}/>\n"
f"{ind} <inertia{mm.group('i')}/>\n"
f"{ind}</inertial>"
)
block, n = _TRIPLE.subn(repl, block)
report.wrapped_inertials += n
# --- 问题二：零质量 link ---
lm = re.match(r'<link\b[^>]*name\s*=\s*"([^"]*)"', block)
if lm:
link_name = lm.group(1)
block = _fix_zero_mass(block, report, link_name)
parts.append(block)
pos = m.end()
parts.append(urdf[pos:])
return "".join(parts), report
def normalize_file(src: str, dst: str) -> NormalizeReport:
"""读文件 → 归一化 → 写文件。"""
with open(src, "r", encoding="utf-8") as fh:
text = fh.read()
out, report = normalize_urdf(text)
with open(dst, "w", encoding="utf-8") as fh:
fh.write(out)
return report
def main(argv: List[str] | None = None) -> int:
"""命令行入口：只看归一化做了什么，不启动 Gazebo。
排查"Gazebo 说我的 link 不存在"时，先跑这个比直接起仿真快得多：
ros2 run arm_sim_gazebo urdf_normalize <urdf> [输出路径]
"""
import argparse
import sys
parser = argparse.ArgumentParser(
prog="urdf_normalize",
description="把 URDF 归一化成 Gazebo 可接受的形态（补 <inertial> 包裹、补零质量）。",
)
parser.add_argument("src", help="输入 URDF 路径")
parser.add_argument(
"dst", nargs="?", help="输出路径（默认写到 stdout，便于直接 grep）"
)
parser.add_argument(
"--quiet", action="store_true", help="不打印归一化摘要"
)
args = parser.parse_args(argv)
with open(args.src, "r", encoding="utf-8") as fh:
text = fh.read()
out, report = normalize_urdf(text)
if not args.quiet:
print(f"[urdf_normalize] {report.summary()}", file=sys.stderr)
if args.dst:
with open(args.dst, "w", encoding="utf-8") as fh:
fh.write(out)
else:
sys.stdout.write(out)
return 0
if __name__ == "__main__":
raise SystemExit(main())
src/arm_sim_gazebo/package.xml
+48
View File
@@ -0,0 +1,48 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>arm_sim_gazebo</name>
<version>0.1.0</version>
<description>
Gazebo 仿真后端。兑现 devices/sim/gazebo/manifest.yaml 里声明的
package: arm_sim_gazebo / executable: gazebo_sim_launcher。
职责：
1. 把 devices/sim/gazebo/urdf 下的仿真描述展开成 URDF，
并归一化成 Gazebo 能接受的形态（见 urdf_normalize.py 的长注释）
2. 起 Gazebo、spawn 机械臂、拉控制器、拉 ros_gz 桥
3. 把应用层需要的接口对齐成与真机/mock 完全一致
（控制器名 rizon_arm_controller、服务与话题名不变），
这样应用层代码不需要知道后端是谁
与真机环境严格隔离：本包只在 ros_gz_sim 环境构建（scripts/build_sim.sh），
真机环境不装它。反过来它也不依赖任何真机专属包。
</description>
<maintainer email="dev@example.com">arm_dev</maintainer>
<license>Apache-2.0</license>
<exec_depend>rclpy</exec_depend>
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
<exec_depend>ament_index_python</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>xacro</exec_depend>
<!-- Gazebo 侧（仅仿真环境有） -->
<exec_depend>ros_gz_sim</exec_depend>
<exec_depend>ros_gz_bridge</exec_depend>
<exec_depend>gz_ros2_control</exec_depend>
<exec_depend>controller_manager</exec_depend>
<exec_depend>joint_state_broadcaster</exec_depend>
<exec_depend>joint_trajectory_controller</exec_depend>
<!-- 描述与参数 -->
<exec_depend>flexiv_description</exec_depend>
<exec_depend>arm_control</exec_depend>
<exec_depend>arm_application</exec_depend>
<test_depend>python3-pytest</test_depend>
<test_depend>python3-yaml</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
src/arm_sim_gazebo/resource/arm_sim_gazebo
View File
src/arm_sim_gazebo/setup.cfg
+12
View File
@@ -0,0 +1,12 @@
[develop]
script_dir=$base/lib/arm_sim_gazebo
[install]
install_scripts=$base/lib/arm_sim_gazebo
[tool:pytest]
# 与 arm_application 同样的处理：Humble 的 launch_testing / launch_ros 两个
# pytest 插件用了旧版 hook 签名，当前 pytest 在加载插件阶段就抛
# PluginValidationError，导致连测试收集都进不去。
# 本包单测不涉及 launch，直接禁掉。
addopts = -p no:launch_testing -p no:launch_ros
testpaths = test
src/arm_sim_gazebo/setup.py
+32
View File
@@ -0,0 +1,32 @@
from setuptools import find_packages, setup
package_name = 'arm_sim_gazebo'
setup(
name=package_name,
version='0.1.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools', 'PyYAML'],
zip_safe=True,
maintainer='arm_dev',
maintainer_email='dev@example.com',
description='Gazebo 仿真后端：描述归一化 + 启动 Gazebo 与 ros2_control 桥接',
license='Apache-2.0',
entry_points={
'console_scripts': [
# 独立小工具：只看归一化做了什么，不启动 Gazebo。
# 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。
'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',
# TODO(P5c-3): manifest 声明的启动器
# devices/sim/gazebo/manifest.yaml -> driver.executable: gazebo_sim_launcher
# 尚未实现，所以这里**先不注册** —— 注册了但模块不存在，
# 构建能过、一运行就 ImportError，不如老实缺着。
# P5c-3 会补上 gazebo_sim_launcher.py 并在此注册。
# 'gazebo_sim_launcher = arm_sim_gazebo.gazebo_sim_launcher:main',
],
},
)
src/arm_sim_gazebo/test/test_urdf_normalize.py
+304
View File
@@ -0,0 +1,304 @@
"""urdf_normalize 的单测。
这些测试锁定的都是**实测踩过的上游缺陷**，不是凭空设想的边界情况：
1. 官方 flexiv_description 的 rizon_arm.xacro 漏了 <inertial> 包裹
→ sdformat 丢掉 link 和它的父关节
2. grav_macro.xacro 里六个 dummy link 质量 0
→ sdformat 惯性叠加除零出 NaN，报的却是父 link
每加一条都对应一个真实现象，所以别把这里的断言当成"实现细节"随手改。
"""
import os
import sys
import pytest
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from arm_sim_gazebo.urdf_normalize import ( # noqa: E402
ZERO_INERTIA_FIX,
ZERO_MASS_FIX_KG,
normalize_urdf,
)
ROBOT_OPEN = (
'<?xml version="1.0"?>\n'
'<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="t">\n'
)
ROBOT_CLOSE = "</robot>\n"
# ---------------------------------------------------------------------------
# 问题一：孤儿 inertial 三件套
# ---------------------------------------------------------------------------
def test_wraps_orphan_triple_in_inertial():
"""漏了 <inertial> 包裹的三件套要被包起来，且一个数字都不能变。"""
urdf = ROBOT_OPEN + (
' <link name="base_link">\n'
' <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.0008 0.0883"/>\n'
' <mass value="3.863"/>\n'
' <inertia ixx="0.0188" ixy="0.0" ixz="0.0" iyy="0.019" iyz="0.0002" izz="0.0103"/>\n'
' <visual name="shell"/>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.wrapped_inertials == 1
assert "<inertial>" in out and "</inertial>" in out
# 数值原样保留 —— 归一化只重新分组，绝不发明物理数据
assert 'xyz="0.0 -0.0008 0.0883"' in out
assert 'value="3.863"' in out
assert 'ixx="0.0188"' in out
assert 'iyz="0.0002"' in out
def test_wrapped_block_is_not_touched():
"""已有 <inertial> 的 link 不能被二次包裹。
这是回归测试：早期用一条全局正则做替换，把夹爪那种本来就正确的
<inertial> 又包了一层，sdformat 报
"Inertial element must have a mass element"，整模型被拒。
"""
urdf = ROBOT_OPEN + (
' <link name="grav_base_link">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0.057"/>\n'
' <mass value="1.186"/>\n'
' <inertia ixx="0.003" ixy="0" ixz="0" iyy="0.003" iyz="0" izz="0.001"/>\n'
' </inertial>\n'
' <visual name="v"/>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.wrapped_inertials == 0
assert out.count("<inertial>") == 1
assert out == urdf, "已规范的 link 应逐字节不变"
def test_link_without_any_inertial_data_is_left_alone():
"""纯坐标系 link（没有 inertial 也没有三件套）不该被动手，
更不该被凭空补出一个 inertial。"""
urdf = ROBOT_OPEN + ' <link name="tool0"/>\n' + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.wrapped_inertials == 0
assert "<inertial>" not in out
assert out == urdf
def test_indentation_is_preserved():
"""包裹后的缩进要跟原文件一致，否则 diff 里全是噪声。"""
urdf = ROBOT_OPEN + (
' <link name="l1">\n'
' <origin xyz="0 0 0" rpy="0 0 0"/>\n'
' <mass value="1.0"/>\n'
' <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>\n'
' </link>\n'
) + ROBOT_CLOSE
out, _ = normalize_urdf(urdf)
assert " <inertial>" in out
assert " <mass" in out # 内部再进一级
# ---------------------------------------------------------------------------
# 问题二：零质量 / 零惯量
# ---------------------------------------------------------------------------
def test_zero_mass_link_gets_tiny_positive_mass():
"""质量 0 的 dummy link 要补成极小正值，否则 sdformat 惯性叠加出 NaN。"""
urdf = ROBOT_OPEN + (
' <link name="left_finger_tip">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="0"/>\n'
' <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.fixed_zero_mass == ["left_finger_tip"]
assert f'<mass value="{ZERO_MASS_FIX_KG}"/>' in out
assert 'mass value="0"' not in out
def test_zero_inertia_tensor_gets_minimal_diagonal():
"""惯量全 0 的张量要补极小对角值，避免退化。"""
urdf = ROBOT_OPEN + (
' <link name="dummy">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="0"/>\n'
' <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.fixed_zero_inertia == ["dummy"]
assert f'ixx="{ZERO_INERTIA_FIX}"' in out
assert f'iyy="{ZERO_INERTIA_FIX}"' in out
assert f'izz="{ZERO_INERTIA_FIX}"' in out
def test_positive_mass_is_never_rewritten():
"""有正常质量的 link 必须逐字节不变 —— 归一化不能偷偷改物理参数。"""
urdf = ROBOT_OPEN + (
' <link name="link1">\n'
' <origin rpy="0 0 0" xyz="0 0 0.1451"/>\n'
' <mass value="3.85"/>\n'
' <inertia ixx="0.0308" ixy="0" ixz="0" iyy="0.031" iyz="0" izz="0.0051"/>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.wrapped_inertials == 1 # 只包了外层
assert rep.fixed_zero_mass == []
assert rep.fixed_zero_inertia == []
assert 'value="3.85"' in out
assert 'ixx="0.0308"' in out
def test_nonzero_offdiagonal_inertia_is_preserved():
"""张量只有部分分量为 0 时不能补 —— 那不是退化张量。"""
urdf = ROBOT_OPEN + (
' <link name="l">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="0"/>\n'
' <inertia ixx="0.01" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.fixed_zero_mass == ["l"]
assert rep.fixed_zero_inertia == [], "ixx 非零，不是退化张量"
assert 'ixx="0.01"' in out
# ---------------------------------------------------------------------------
# 性质
# ---------------------------------------------------------------------------
def test_normalize_is_idempotent():
"""跑两遍等于跑一遍。否则启动器里任何一次重复调用都会把 URDF 越改越乱。"""
urdf = ROBOT_OPEN + (
' <link name="base_link">\n'
' <origin rpy="0 0 0" xyz="0 0 0.0883"/>\n'
' <mass value="3.863"/>\n'
' <inertia ixx="0.0188" ixy="0" ixz="0" iyy="0.019" iyz="0" izz="0.0103"/>\n'
' </link>\n'
' <link name="tip">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="0"/>\n'
' <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
once, rep1 = normalize_urdf(urdf)
twice, rep2 = normalize_urdf(once)
assert once == twice
assert rep1.changed and not rep2.changed
def test_xml_stays_well_formed_after_normalize():
"""归一化后必须是合法 XML —— 这是 sdformat 的第一道关。"""
import xml.etree.ElementTree as ET
urdf = ROBOT_OPEN + (
' <link name="l1">\n'
' <origin xyz="0 0 0" rpy="0 0 0"/>\n'
' <mass value="1.5"/>\n'
' <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>\n'
' </link>\n'
' <joint name="j1" type="fixed">\n'
' <parent link="l1"/><child link="l2"/>\n'
' </joint>\n'
' <link name="l2"/>\n'
) + ROBOT_CLOSE
out, _ = normalize_urdf(urdf)
ET.fromstring(out) # 抛异常即失败
def test_report_summary_is_human_readable():
"""摘要要能直接打印给用户看，说明改了什么。"""
urdf = ROBOT_OPEN + (
' <link name="a">\n'
' <origin xyz="0 0 0" rpy="0 0 0"/>\n'
' <mass value="0"/>\n'
' <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </link>\n'
) + ROBOT_CLOSE
_, rep = normalize_urdf(urdf)
s = rep.summary()
assert "补 <inertial> 包裹 1 处" in s
assert "a" in s
def test_clean_urdf_reports_no_change():
urdf = ROBOT_OPEN + (
' <link name="l">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="2.0"/>\n'
' <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
_, rep = normalize_urdf(urdf)
assert not rep.changed
assert rep.summary() == "URDF 无需归一化"
@pytest.mark.parametrize("mass_text", ["0", "0.0", "0.000"])
def test_all_zero_mass_spellings_are_caught(mass_text):
"""上游可能把 0 写成 0 / 0.0 / 0.000，都要认出来。"""
urdf = ROBOT_OPEN + (
' <link name="l">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
f' <mass value="{mass_text}"/>\n'
' <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
_, rep = normalize_urdf(urdf)
assert rep.fixed_zero_mass == ["l"]
def test_malformed_mass_value_does_not_crash():
"""数值解析失败时不能崩，也不能乱改。"""
urdf = ROBOT_OPEN + (
' <link name="l">\n'
' <inertial>\n'
' <origin rpy="0 0 0" xyz="0 0 0"/>\n'
' <mass value="not-a-number"/>\n'
' <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>\n'
' </inertial>\n'
' </link>\n'
) + ROBOT_CLOSE
out, rep = normalize_urdf(urdf)
assert rep.fixed_zero_mass == []
assert 'value="not-a-number"' in out