别人克隆后没有 install，source 会直接失败。把展开后的 install 放进仓库，符号链接和指向本机 build 的 egg-link 收成真实文件，Python 入口改走当前环境的解释器，这样不用先 colcon build 也能启动。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python3
|
||||
# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','gazebo_sim_launcher'
|
||||
import re
|
||||
import sys
|
||||
|
||||
# for compatibility with easy_install; see #2198
|
||||
__requires__ = 'arm_sim_gazebo==0.1.0'
|
||||
|
||||
try:
|
||||
from importlib.metadata import distribution
|
||||
except ImportError:
|
||||
try:
|
||||
from importlib_metadata import distribution
|
||||
except ImportError:
|
||||
from pkg_resources import load_entry_point
|
||||
|
||||
|
||||
def importlib_load_entry_point(spec, group, name):
|
||||
dist_name, _, _ = spec.partition('==')
|
||||
matches = (
|
||||
entry_point
|
||||
for entry_point in distribution(dist_name).entry_points
|
||||
if entry_point.group == group and entry_point.name == name
|
||||
)
|
||||
return next(matches).load()
|
||||
|
||||
|
||||
globals().setdefault('load_entry_point', importlib_load_entry_point)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
|
||||
sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'gazebo_sim_launcher')())
|
||||
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python3
|
||||
# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','sim_camera_adapter'
|
||||
import re
|
||||
import sys
|
||||
|
||||
# for compatibility with easy_install; see #2198
|
||||
__requires__ = 'arm_sim_gazebo==0.1.0'
|
||||
|
||||
try:
|
||||
from importlib.metadata import distribution
|
||||
except ImportError:
|
||||
try:
|
||||
from importlib_metadata import distribution
|
||||
except ImportError:
|
||||
from pkg_resources import load_entry_point
|
||||
|
||||
|
||||
def importlib_load_entry_point(spec, group, name):
|
||||
dist_name, _, _ = spec.partition('==')
|
||||
matches = (
|
||||
entry_point
|
||||
for entry_point in distribution(dist_name).entry_points
|
||||
if entry_point.group == group and entry_point.name == name
|
||||
)
|
||||
return next(matches).load()
|
||||
|
||||
|
||||
globals().setdefault('load_entry_point', importlib_load_entry_point)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
|
||||
sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'sim_camera_adapter')())
|
||||
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python3
|
||||
# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','sim_gripper_adapter'
|
||||
import re
|
||||
import sys
|
||||
|
||||
# for compatibility with easy_install; see #2198
|
||||
__requires__ = 'arm_sim_gazebo==0.1.0'
|
||||
|
||||
try:
|
||||
from importlib.metadata import distribution
|
||||
except ImportError:
|
||||
try:
|
||||
from importlib_metadata import distribution
|
||||
except ImportError:
|
||||
from pkg_resources import load_entry_point
|
||||
|
||||
|
||||
def importlib_load_entry_point(spec, group, name):
|
||||
dist_name, _, _ = spec.partition('==')
|
||||
matches = (
|
||||
entry_point
|
||||
for entry_point in distribution(dist_name).entry_points
|
||||
if entry_point.group == group and entry_point.name == name
|
||||
)
|
||||
return next(matches).load()
|
||||
|
||||
|
||||
globals().setdefault('load_entry_point', importlib_load_entry_point)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
|
||||
sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'sim_gripper_adapter')())
|
||||
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python3
|
||||
# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','sim_home_service'
|
||||
import re
|
||||
import sys
|
||||
|
||||
# for compatibility with easy_install; see #2198
|
||||
__requires__ = 'arm_sim_gazebo==0.1.0'
|
||||
|
||||
try:
|
||||
from importlib.metadata import distribution
|
||||
except ImportError:
|
||||
try:
|
||||
from importlib_metadata import distribution
|
||||
except ImportError:
|
||||
from pkg_resources import load_entry_point
|
||||
|
||||
|
||||
def importlib_load_entry_point(spec, group, name):
|
||||
dist_name, _, _ = spec.partition('==')
|
||||
matches = (
|
||||
entry_point
|
||||
for entry_point in distribution(dist_name).entry_points
|
||||
if entry_point.group == group and entry_point.name == name
|
||||
)
|
||||
return next(matches).load()
|
||||
|
||||
|
||||
globals().setdefault('load_entry_point', importlib_load_entry_point)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
|
||||
sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'sim_home_service')())
|
||||
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python3
|
||||
# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','sim_tactile_adapter'
|
||||
import re
|
||||
import sys
|
||||
|
||||
# for compatibility with easy_install; see #2198
|
||||
__requires__ = 'arm_sim_gazebo==0.1.0'
|
||||
|
||||
try:
|
||||
from importlib.metadata import distribution
|
||||
except ImportError:
|
||||
try:
|
||||
from importlib_metadata import distribution
|
||||
except ImportError:
|
||||
from pkg_resources import load_entry_point
|
||||
|
||||
|
||||
def importlib_load_entry_point(spec, group, name):
|
||||
dist_name, _, _ = spec.partition('==')
|
||||
matches = (
|
||||
entry_point
|
||||
for entry_point in distribution(dist_name).entry_points
|
||||
if entry_point.group == group and entry_point.name == name
|
||||
)
|
||||
return next(matches).load()
|
||||
|
||||
|
||||
globals().setdefault('load_entry_point', importlib_load_entry_point)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
|
||||
sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'sim_tactile_adapter')())
|
||||
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python3
|
||||
# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','sim_truth_perception'
|
||||
import re
|
||||
import sys
|
||||
|
||||
# for compatibility with easy_install; see #2198
|
||||
__requires__ = 'arm_sim_gazebo==0.1.0'
|
||||
|
||||
try:
|
||||
from importlib.metadata import distribution
|
||||
except ImportError:
|
||||
try:
|
||||
from importlib_metadata import distribution
|
||||
except ImportError:
|
||||
from pkg_resources import load_entry_point
|
||||
|
||||
|
||||
def importlib_load_entry_point(spec, group, name):
|
||||
dist_name, _, _ = spec.partition('==')
|
||||
matches = (
|
||||
entry_point
|
||||
for entry_point in distribution(dist_name).entry_points
|
||||
if entry_point.group == group and entry_point.name == name
|
||||
)
|
||||
return next(matches).load()
|
||||
|
||||
|
||||
globals().setdefault('load_entry_point', importlib_load_entry_point)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
|
||||
sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'sim_truth_perception')())
|
||||
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/env python3
|
||||
# EASY-INSTALL-ENTRY-SCRIPT: 'arm_sim_gazebo==0.1.0','console_scripts','urdf_normalize'
|
||||
import re
|
||||
import sys
|
||||
|
||||
# for compatibility with easy_install; see #2198
|
||||
__requires__ = 'arm_sim_gazebo==0.1.0'
|
||||
|
||||
try:
|
||||
from importlib.metadata import distribution
|
||||
except ImportError:
|
||||
try:
|
||||
from importlib_metadata import distribution
|
||||
except ImportError:
|
||||
from pkg_resources import load_entry_point
|
||||
|
||||
|
||||
def importlib_load_entry_point(spec, group, name):
|
||||
dist_name, _, _ = spec.partition('==')
|
||||
matches = (
|
||||
entry_point
|
||||
for entry_point in distribution(dist_name).entry_points
|
||||
if entry_point.group == group and entry_point.name == name
|
||||
)
|
||||
return next(matches).load()
|
||||
|
||||
|
||||
globals().setdefault('load_entry_point', importlib_load_entry_point)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
|
||||
sys.exit(load_entry_point('arm_sim_gazebo==0.1.0', 'console_scripts', 'urdf_normalize')())
|
||||
+14
@@ -0,0 +1,14 @@
|
||||
Metadata-Version: 2.4
|
||||
Name: arm_sim_gazebo
|
||||
Version: 0.1.0
|
||||
Summary: Gazebo 仿真后端：描述归一化 + 启动 Gazebo 与 ros2_control 桥接
|
||||
Maintainer: arm_dev
|
||||
Maintainer-email: dev@example.com
|
||||
License: Apache-2.0
|
||||
Requires-Dist: setuptools
|
||||
Requires-Dist: PyYAML
|
||||
Dynamic: license
|
||||
Dynamic: maintainer
|
||||
Dynamic: maintainer-email
|
||||
Dynamic: requires-dist
|
||||
Dynamic: summary
|
||||
+43
@@ -0,0 +1,43 @@
|
||||
package.xml
|
||||
setup.cfg
|
||||
setup.py
|
||||
../../build/arm_sim_gazebo/arm_sim_gazebo.egg-info/PKG-INFO
|
||||
../../build/arm_sim_gazebo/arm_sim_gazebo.egg-info/SOURCES.txt
|
||||
../../build/arm_sim_gazebo/arm_sim_gazebo.egg-info/dependency_links.txt
|
||||
../../build/arm_sim_gazebo/arm_sim_gazebo.egg-info/entry_points.txt
|
||||
../../build/arm_sim_gazebo/arm_sim_gazebo.egg-info/requires.txt
|
||||
../../build/arm_sim_gazebo/arm_sim_gazebo.egg-info/top_level.txt
|
||||
../../build/arm_sim_gazebo/arm_sim_gazebo.egg-info/zip-safe
|
||||
arm_sim_gazebo/__init__.py
|
||||
arm_sim_gazebo/gazebo_sim_launcher.py
|
||||
arm_sim_gazebo/robot_description.py
|
||||
arm_sim_gazebo/sim_camera_adapter.py
|
||||
arm_sim_gazebo/sim_cameras.py
|
||||
arm_sim_gazebo/sim_grasp_lock.py
|
||||
arm_sim_gazebo/sim_gripper.py
|
||||
arm_sim_gazebo/sim_gripper_adapter.py
|
||||
arm_sim_gazebo/sim_gripper_kin.py
|
||||
arm_sim_gazebo/sim_home_service.py
|
||||
arm_sim_gazebo/sim_scene.py
|
||||
arm_sim_gazebo/sim_site.py
|
||||
arm_sim_gazebo/sim_tactile.py
|
||||
arm_sim_gazebo/sim_tactile_adapter.py
|
||||
arm_sim_gazebo/sim_truth_perception.py
|
||||
arm_sim_gazebo/sim_wrist_ft.py
|
||||
arm_sim_gazebo/truth_holes.py
|
||||
arm_sim_gazebo/urdf_normalize.py
|
||||
launch/gazebo_sim.launch.py
|
||||
resource/arm_sim_gazebo
|
||||
test/test_check_sim_scene.py
|
||||
test/test_repo_root.py
|
||||
test/test_robot_description.py
|
||||
test/test_sim_cameras.py
|
||||
test/test_sim_grasp_lock.py
|
||||
test/test_sim_gripper.py
|
||||
test/test_sim_gripper_kin.py
|
||||
test/test_sim_scene.py
|
||||
test/test_sim_site.py
|
||||
test/test_sim_tactile.py
|
||||
test/test_sim_truth_perception.py
|
||||
test/test_truth_holes.py
|
||||
test/test_urdf_normalize.py
|
||||
+1
@@ -0,0 +1 @@
|
||||
|
||||
+8
@@ -0,0 +1,8 @@
|
||||
[console_scripts]
|
||||
gazebo_sim_launcher = arm_sim_gazebo.gazebo_sim_launcher:main
|
||||
sim_camera_adapter = arm_sim_gazebo.sim_camera_adapter:main
|
||||
sim_gripper_adapter = arm_sim_gazebo.sim_gripper_adapter:main
|
||||
sim_home_service = arm_sim_gazebo.sim_home_service:main
|
||||
sim_tactile_adapter = arm_sim_gazebo.sim_tactile_adapter:main
|
||||
sim_truth_perception = arm_sim_gazebo.sim_truth_perception:main
|
||||
urdf_normalize = arm_sim_gazebo.urdf_normalize:main
|
||||
+2
@@ -0,0 +1,2 @@
|
||||
setuptools
|
||||
PyYAML
|
||||
+1
@@ -0,0 +1 @@
|
||||
arm_sim_gazebo
|
||||
+1
@@ -0,0 +1 @@
|
||||
|
||||
@@ -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"]
|
||||
+221
@@ -0,0 +1,221 @@
|
||||
"""gazebo_sim_launcher —— devices/sim/gazebo/manifest.yaml 声明的那个可执行。
|
||||
|
||||
职责刻意做得很薄：它只负责"准备描述"这件事，然后交给常规的 launch 文件去跑。
|
||||
不把整套启动逻辑塞进可执行文件里，是因为 `ros2 launch` 的日志/退出码/中断处理
|
||||
都更成熟，自己重造一份只会更难调试。
|
||||
|
||||
用法：
|
||||
gazebo_sim_launcher # 准备描述并启动仿真
|
||||
gazebo_sim_launcher --print-urdf # 只看展开+归一化后的 URDF（排查用）
|
||||
gazebo_sim_launcher --check # 只做自检，不启动任何东西
|
||||
gazebo_sim_launcher --headless false # 带 GUI
|
||||
gazebo_sim_launcher --dry-run # 打印将要执行的命令，不执行
|
||||
|
||||
为什么 --print-urdf / --check 值得单独做：仿真跑不起来时，绝大多数问题都在
|
||||
"描述对不对"这一层（link 被丢、关节名对不上）。能把描述单独打出来看，
|
||||
比对着整套仿真的日志找要快一个量级。这两个选项都不需要 Gazebo 在跑。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
from .robot_description import (
|
||||
SimDescriptionError,
|
||||
check_base_plugins,
|
||||
default_controllers,
|
||||
default_world,
|
||||
default_xacro,
|
||||
prepare_urdf,
|
||||
repo_root,
|
||||
world_name,
|
||||
)
|
||||
from .sim_site import (
|
||||
SimSiteError,
|
||||
load_sim_site,
|
||||
physics_enabled_in_world,
|
||||
resolve_world,
|
||||
unwired_warnings,
|
||||
)
|
||||
|
||||
LAUNCH_PKG = "arm_sim_gazebo"
|
||||
LAUNCH_FILE = "gazebo_sim.launch.py"
|
||||
|
||||
# 仿真实际使用的关节前缀，用来跟 site.yaml 的 sim.joint_prefix 比对。
|
||||
# 它由 arm.sim.urdf.xacro 的 arm_prefix 默认值决定，并且 controllers.yaml
|
||||
# 里硬编码了 sim_joint1..7，所以暂时**不可配置** —— 改了这里不改那两处会散架。
|
||||
# site.yaml 若写了一个不一样的值，unwired_warnings() 会告警，而不是静默忽略。
|
||||
ACTUAL_JOINT_PREFIX = "sim_"
|
||||
|
||||
|
||||
def _build_launch_cmd(extra: List[str]) -> List[str]:
|
||||
# 用 python -m 调 ros2 而不是 exec "ros2"：这样在 ros2 CLI 不在 PATH 的
|
||||
# 环境里也能给出清晰报错，而不是一句 "No such file or directory"。
|
||||
return ["ros2", "launch", LAUNCH_PKG, LAUNCH_FILE, *extra]
|
||||
|
||||
|
||||
def main(argv: Optional[List[str]] = None) -> int:
|
||||
parser = argparse.ArgumentParser(
|
||||
prog="gazebo_sim_launcher",
|
||||
description="准备仿真描述并启动 Gazebo（arm_sim_gazebo）。",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--print-urdf",
|
||||
action="store_true",
|
||||
help="把展开并归一化后的 URDF 打到 stdout，不启动仿真",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--check",
|
||||
action="store_true",
|
||||
help="只做自检（能否找到仓库根、描述能否展开、归一化了什么），不启动",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--dry-run",
|
||||
action="store_true",
|
||||
help="打印将要执行的 ros2 launch 命令，不执行",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-gripper",
|
||||
action="store_true",
|
||||
help="不挂夹爪（纯运动学验证时更快）",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--no-sensors",
|
||||
action="store_true",
|
||||
help="不挂相机与触觉",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--world", default="", help="世界文件路径，覆盖默认的 assembly.world"
|
||||
)
|
||||
parser.add_argument(
|
||||
"--headless",
|
||||
default="true",
|
||||
choices=["true", "false"],
|
||||
help="true=只开 Gazebo 服务端（无显示器环境用）；false=带 GUI",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--verbose",
|
||||
action="store_true",
|
||||
help="打开 Gazebo 详细日志（排查插件加载失败时开）",
|
||||
)
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
# ---- 1) 准备描述（这一步是真正有内容的，其余都是转发）------------------
|
||||
try:
|
||||
root = repo_root()
|
||||
xacro_path = default_xacro(root)
|
||||
controllers = default_controllers(root)
|
||||
# world 的优先级：--world > site.yaml 的 sim.world > 内置默认。
|
||||
# 以前这里只认 --world 和硬编码默认值，而 site.yaml / manifest.yaml
|
||||
# 都宣称改 sim.world 就生效 —— 结果改了毫无反应且不报错。
|
||||
world, world_origin = resolve_world(args.world, root, fallback=default_world(root))
|
||||
world = str(world)
|
||||
site = load_sim_site(root)
|
||||
urdf, report = prepare_urdf(
|
||||
xacro_path=xacro_path,
|
||||
controllers_file=controllers,
|
||||
load_gripper=not args.no_gripper,
|
||||
with_sensors=not args.no_sensors,
|
||||
)
|
||||
wname = world_name(world)
|
||||
missing = check_base_plugins(Path(world))
|
||||
except (SimDescriptionError, SimSiteError) as exc:
|
||||
print(f"[gazebo_sim_launcher] 失败：\n{exc}", file=sys.stderr)
|
||||
return 2
|
||||
|
||||
# site.yaml 里"声明了但没接线"的键：与其静默无效，不如明说。
|
||||
# 见 sim_site.unwired_warnings() —— 那里逐条说明了为什么还没接。
|
||||
for _w in unwired_warnings(
|
||||
site,
|
||||
actual_joint_prefix=ACTUAL_JOINT_PREFIX,
|
||||
physics_in_world=physics_enabled_in_world(Path(world)),
|
||||
):
|
||||
print(f"[gazebo_sim_launcher] ⚠ {_w}", file=sys.stderr)
|
||||
|
||||
if missing:
|
||||
# 这一条能省掉一整个下午：world 漏声明基础系统时，仿真看起来完全正常
|
||||
# （/clock 在走、create 打印成功），但机械臂从未进入 Gazebo。
|
||||
print(
|
||||
"[gazebo_sim_launcher] ⚠ world 缺少基础系统: "
|
||||
+ ", ".join(missing) + "\n"
|
||||
" world 里只要声明了任何一个 <plugin>，Gazebo 就不再加载默认系统列表。\n"
|
||||
" 缺 UserCommands → /world/<名的>/create 服务不存在 → 模型根本不会\n"
|
||||
" 被创建，而 create 仍会打印 'OK creation of entity'。最终表现为\n"
|
||||
" 控制器永远拉不起来。修法：在这个 world 里显式声明\n"
|
||||
" Physics / UserCommands / SceneBroadcaster（见文件里的注释）。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
|
||||
if args.print_urdf:
|
||||
# URDF 走 stdout（可重定向），摘要走 stderr（不污染管道）
|
||||
sys.stdout.write(urdf)
|
||||
print(f"\n[gazebo_sim_launcher] {report.summary()}", file=sys.stderr)
|
||||
return 0
|
||||
|
||||
if args.check:
|
||||
print("[gazebo_sim_launcher] 自检通过")
|
||||
print(f" 仓库根 : {root}")
|
||||
print(f" 描述 : {xacro_path}")
|
||||
print(f" 控制器配置 : {controllers}")
|
||||
print(f" 世界 : {world} (world 名: {wname})")
|
||||
print(f" 世界来源 : {world_origin}")
|
||||
if site.source is not None:
|
||||
print(f" site.yaml : {site.source}")
|
||||
else:
|
||||
print(" site.yaml : 不存在（用默认值；这不会影响仿真）")
|
||||
print(f" URDF 归一化 : {report.summary()}")
|
||||
if missing:
|
||||
print(
|
||||
f" ⚠ world 缺少基础系统: {', '.join(missing)}\n"
|
||||
" world 里只要声明了任何一个 <plugin>，Gazebo 就不再加载默认系统。\n"
|
||||
" 缺 UserCommands → /world/<名的>/create 服务不存在 → 模型不会被创建，\n"
|
||||
" 而 create 仍打印 'OK creation of entity'，控制器永远拉不起来。"
|
||||
)
|
||||
else:
|
||||
print(" world 基础系统 : Physics / UserCommands / SceneBroadcaster 齐备")
|
||||
if not report.changed:
|
||||
print(
|
||||
" ⚠ 归一化没有改动任何内容。若后续 Gazebo 报 link 不存在，\n"
|
||||
" 先确认 flexiv_description 的版本是否已自行修好那两个上游缺陷。"
|
||||
)
|
||||
print("\n 下一步: gazebo_sim_launcher --dry-run # 看会执行什么")
|
||||
return 0
|
||||
|
||||
# ---- 2) 转发给 launch 文件 ---------------------------------------------
|
||||
# 参数名在这里显式映射到 launch 参数，不做通用透传 —— 通用透传会把拼错
|
||||
# 的参数名静默吞掉（ros2 launch 对未知的 key:=value 不报错），
|
||||
# 而"我明明设了 --headless false 却没生效"这种问题极难查。
|
||||
extra = [
|
||||
"headless:=" + args.headless,
|
||||
"verbose:=" + ("true" if args.verbose else "false"),
|
||||
]
|
||||
if args.world:
|
||||
extra.append("world:=" + args.world)
|
||||
if args.no_gripper:
|
||||
extra.append("load_gripper:=false")
|
||||
if args.no_sensors:
|
||||
extra.append("with_sensors:=false")
|
||||
|
||||
cmd = _build_launch_cmd(extra)
|
||||
print(f"[gazebo_sim_launcher] {report.summary()}")
|
||||
print(f"[gazebo_sim_launcher] exec: {' '.join(cmd)}")
|
||||
|
||||
if args.dry_run:
|
||||
return 0
|
||||
|
||||
try:
|
||||
os.execvp(cmd[0], cmd)
|
||||
except FileNotFoundError:
|
||||
print(
|
||||
"[gazebo_sim_launcher] 找不到 ros2 命令。先激活环境：\n"
|
||||
" source scripts/sim_env.sh",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 2
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
+612
@@ -0,0 +1,612 @@
|
||||
"""组装仿真用的机器人描述：xacro 展开 → 归一化 → 交给 ROS / Gazebo。
|
||||
|
||||
=============================================================================
|
||||
为什么单独一个模块
|
||||
=============================================================================
|
||||
"把 devices/sim/gazebo/urdf 下的描述变成一份可用的 URDF"这件事有两处需要：
|
||||
1. launch 文件（真正启动仿真时）
|
||||
2. gazebo_sim_launcher 的 --print-urdf（只想看看展开+归一化后长什么样）
|
||||
两边各写一份必然漂移，所以实现只放这里，两处都 import 它。
|
||||
|
||||
=============================================================================
|
||||
仓库根怎么找
|
||||
=============================================================================
|
||||
devices/ **不是 ROS 包**，所以 $(find ...) 找不到它，必须用文件系统路径。
|
||||
按可靠性从高到低试三种：
|
||||
1. $ROS2_WS（scripts/sim_env.sh 会导出）—— 最可靠，显式且便宜
|
||||
2. 从当前工作目录向上找标志文件 —— 手工跑脚本时的常见情形
|
||||
3. 从本模块所在位置向上找 —— 兜底（安装到 install_sim/ 后路径较深）
|
||||
三者都失败时给出明确的报错和修复提示，而不是抛一个看不懂的 FileNotFoundError。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import os
|
||||
import re
|
||||
import subprocess
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Sequence, Tuple
|
||||
|
||||
from .urdf_normalize import NormalizeReport, normalize_urdf
|
||||
|
||||
# 用它判断"这个目录是不是仓库根"。选它是因为这个文件是仿真描述的唯一入口，
|
||||
# 存在即说明目录结构正确；不像 .git 那样在导出/打包后可能不存在。
|
||||
_MARKER = "devices/sim/gazebo/urdf/arm.sim.urdf.xacro"
|
||||
|
||||
_XACRO_REL = "devices/sim/gazebo/urdf/arm.sim.urdf.xacro"
|
||||
_CONTROLLERS_REL = "devices/sim/gazebo/config/controllers.yaml"
|
||||
_WORLD_REL = "devices/sim/gazebo/worlds/assembly.world"
|
||||
|
||||
|
||||
class SimDescriptionError(RuntimeError):
|
||||
"""描述准备过程中的可预期失败，消息面向使用者而非开发者。"""
|
||||
|
||||
|
||||
def repo_root(start: Optional[Path] = None) -> Path:
|
||||
"""定位仓库根。找不到时抛 SimDescriptionError 并说明怎么修。"""
|
||||
env = os.environ.get("ROS2_WS", "").strip()
|
||||
if env:
|
||||
p = Path(env).expanduser()
|
||||
if (p / _MARKER).is_file():
|
||||
return p
|
||||
|
||||
seeds = []
|
||||
if start is not None:
|
||||
seeds.append(Path(start).expanduser())
|
||||
seeds.append(Path.cwd())
|
||||
seeds.append(Path(__file__).resolve().parent)
|
||||
|
||||
for seed in seeds:
|
||||
cur = seed.resolve()
|
||||
for _ in range(12): # 足够覆盖仓库内的任意深度
|
||||
if (cur / _MARKER).is_file():
|
||||
return cur
|
||||
if cur.parent == cur:
|
||||
break
|
||||
cur = cur.parent
|
||||
|
||||
raise SimDescriptionError(
|
||||
"找不到仓库根（即包含 %s 的目录）。\n"
|
||||
" 修法（任选其一）：\n"
|
||||
" 1) source scripts/sim_env.sh # 它会导出 ROS2_WS\n"
|
||||
" 2) 在仓库根目录下执行\n"
|
||||
" 已试过的起点：%s" % (_MARKER, ", ".join(str(s) for s in seeds))
|
||||
)
|
||||
|
||||
|
||||
def default_xacro(root: Optional[Path] = None) -> Path:
|
||||
return (root or repo_root()) / _XACRO_REL
|
||||
|
||||
|
||||
def default_controllers(root: Optional[Path] = None) -> Path:
|
||||
return (root or repo_root()) / _CONTROLLERS_REL
|
||||
|
||||
|
||||
def default_world(root: Optional[Path] = None) -> Path:
|
||||
return (root or repo_root()) / _WORLD_REL
|
||||
|
||||
|
||||
def world_name(world_file: Path) -> str:
|
||||
"""从 world 文件里读出 <world name="...">。
|
||||
|
||||
为什么解析而不是让调用方传：world 名必须和文件内容一致，让它们分别配置
|
||||
就等于制造一个"改了文件名忘了改参数"的机会。create 参数写错 world 名时
|
||||
Gazebo 不会报错，只是实体静默地没被创建。
|
||||
"""
|
||||
try:
|
||||
text = Path(world_file).read_text(encoding="utf-8", errors="replace")
|
||||
except OSError as exc:
|
||||
raise SimDescriptionError(f"读不到 world 文件 {world_file}: {exc}") from exc
|
||||
|
||||
# 同时接受单引号与双引号：手写 world 时两种都会出现，
|
||||
# 早期只认双引号，于是 `<world name='x'>` 会被判成"找不到 world 名"。
|
||||
m = re.search(r"""<world\b[^>]*\bname\s*=\s*["']([^"']+)["']""", text)
|
||||
if not m:
|
||||
raise SimDescriptionError(
|
||||
f"{world_file} 里找不到 <world name=\"...\">，无法确定 world 名。"
|
||||
)
|
||||
return m.group(1)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 从 world 文件里读模型位姿
|
||||
# =============================================================================
|
||||
# 为什么需要：Gazebo 里的 <model>（尤其是 static 的工装/相机支架）**不会**产生
|
||||
# ROS TF —— TF 只来自 robot_state_publisher。但应用要做 tf2 查询，比如
|
||||
# "把 D435 看到的点从相机系转到 world"。少了这条 TF，查询直接报
|
||||
# "frame does not exist"，而图像本身看起来完全正常。
|
||||
#
|
||||
# 为什么是"解析"而不是"再配置一遍"：位姿的唯一真相就在 world 文件里。
|
||||
# 另配一份等于制造"改了 world 忘了改配置"的机会，而这种不一致不会报错，
|
||||
# 只会让相机看到的东西和它自称的位置对不上 —— 极难查。
|
||||
def _find_block(text: str, tag: str, name: str) -> Optional[str]:
|
||||
"""取出 <tag name="name">...</tag> 的内容（考虑同名标签嵌套）。
|
||||
|
||||
SDF 里 <model> 可以嵌套 <model>（组合体），<link> 里也可能有 <link>（少见）。
|
||||
只找第一个 </tag> 在嵌套时会截断到错误的位置，所以这里数嵌套深度。
|
||||
"""
|
||||
pat = re.compile(
|
||||
r"<%(t)s\b[^>]*\bname\s*=\s*[\"']%(n)s[\"'][^>]*>" % {"t": tag, "n": re.escape(name)}
|
||||
)
|
||||
m = pat.search(text)
|
||||
if not m:
|
||||
return None
|
||||
|
||||
# 自闭合：<link name="l"/> 是完全合法的 SDF（没有子元素）。
|
||||
# 不特判的话会走到下面的"找 </link>"分支、找不到 → 返回 None，
|
||||
# 于是被误报成"这个 link 不存在"，而它明明在。
|
||||
if m.group(0).rstrip().endswith("/>"):
|
||||
return ""
|
||||
|
||||
inner_start = m.end()
|
||||
depth = 1
|
||||
pos = inner_start
|
||||
open_re = re.compile(r"<%(t)s\b" % {"t": tag})
|
||||
close_re = re.compile(r"</%(t)s\s*>" % {"t": tag})
|
||||
while depth > 0:
|
||||
o = open_re.search(text, pos)
|
||||
c = close_re.search(text, pos)
|
||||
if c is None:
|
||||
return None # 标签没闭合 → 文件本身有问题，交给别的检查去报
|
||||
if o is not None and o.start() < c.start():
|
||||
depth += 1
|
||||
pos = o.end()
|
||||
else:
|
||||
depth -= 1
|
||||
if depth == 0:
|
||||
return text[inner_start : c.start()]
|
||||
pos = c.end()
|
||||
return None
|
||||
|
||||
|
||||
def _own_pose(block: str, stop_before: Tuple[str, ...]) -> Tuple[float, float, float, float, float, float]:
|
||||
"""取一段块自己的 <pose>（默认全 0）。
|
||||
|
||||
只认**第一个** <pose>，且必须出现在 stop_before 里任一标签之前 ——
|
||||
这样才不会把子元素的 pose 误当成父元素的。例如 <model> 的 pose 必须在
|
||||
第一个 <link>/<model>/<joint> 之前；<link> 的 pose 必须在第一个
|
||||
<visual>/<collision>/<sensor> 之前。
|
||||
"""
|
||||
cut = len(block)
|
||||
for tag in stop_before:
|
||||
i = block.find("<" + tag)
|
||||
if 0 <= i < cut:
|
||||
cut = i
|
||||
m = re.search(r"<pose(?:\s[^>]*)?>([^<]*)</pose>", block[:cut])
|
||||
if not m:
|
||||
return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||
vals = m.group(1).split()
|
||||
if len(vals) < 6:
|
||||
return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||
try:
|
||||
return tuple(float(v) for v in vals[:6]) # type: ignore[return-value]
|
||||
except ValueError:
|
||||
return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||
|
||||
|
||||
def _rpy_to_quat(r: float, p: float, y: float) -> Tuple[float, float, float, float]:
|
||||
cr, sr = math.cos(r / 2), math.sin(r / 2)
|
||||
cp, sp = math.cos(p / 2), math.sin(p / 2)
|
||||
cy, sy = math.cos(y / 2), math.sin(y / 2)
|
||||
return (
|
||||
sr * cp * cy - cr * sp * sy,
|
||||
cr * sp * cy + sr * cp * sy,
|
||||
cr * cp * sy - sr * sp * cy,
|
||||
cr * cp * cy + sr * sp * sy,
|
||||
)
|
||||
|
||||
|
||||
def _quat_to_rpy(q: Tuple[float, float, float, float]) -> Tuple[float, float, float]:
|
||||
x, y, z, w = q
|
||||
# roll (x 轴)
|
||||
sinr = 2 * (w * x + y * z)
|
||||
cosr = 1 - 2 * (x * x + y * y)
|
||||
roll = math.atan2(sinr, cosr)
|
||||
# pitch (y 轴)：用 asin，并夹到 [-1,1] 免得浮点误差导致 domain error
|
||||
sinp = 2 * (w * y - z * x)
|
||||
sinp = max(-1.0, min(1.0, sinp))
|
||||
pitch = math.asin(sinp)
|
||||
# yaw (z 轴)
|
||||
siny = 2 * (w * z + x * y)
|
||||
cosy = 1 - 2 * (y * y + z * z)
|
||||
yaw = math.atan2(siny, cosy)
|
||||
return (roll, pitch, yaw)
|
||||
|
||||
|
||||
def _quat_mul(a, b):
|
||||
ax, ay, az, aw = a
|
||||
bx, by, bz, bw = b
|
||||
return (
|
||||
aw * bx + ax * bw + ay * bz - az * by,
|
||||
aw * by - ax * bz + ay * bw + az * bx,
|
||||
aw * bz + ax * by - ay * bx + az * bw,
|
||||
aw * bw - ax * bx - ay * by - az * bz,
|
||||
)
|
||||
|
||||
|
||||
def _rotate(q, v):
|
||||
"""用四元数旋转向量 v（v 在子系里，返回在父系里的表达）。"""
|
||||
x, y, z, w = q
|
||||
vx, vy, vz = v
|
||||
# t = 2 * (q_vec × v)
|
||||
tx = 2 * (y * vz - z * vy)
|
||||
ty = 2 * (z * vx - x * vz)
|
||||
tz = 2 * (x * vy - y * vx)
|
||||
# v' = v + w*t + q_vec × t
|
||||
return (
|
||||
vx + w * tx + (y * tz - z * ty),
|
||||
vy + w * ty + (z * tx - x * tz),
|
||||
vz + w * tz + (x * ty - y * tx),
|
||||
)
|
||||
|
||||
|
||||
def model_link_pose(
|
||||
world_file: Path, model: str, link: str = ""
|
||||
) -> Tuple[Tuple[float, float, float], Tuple[float, float, float]]:
|
||||
"""返回 world 文件里 <model name=model> 的 <link name=link> 在**世界系**的位姿。
|
||||
|
||||
返回值是 ((x, y, z), (roll, pitch, yaw))。link 留空表示取模型本身的位姿。
|
||||
两者都会做复合（模型位姿 ∘ link 位姿），因为 SDF 的 <pose> 是相对父元素的。
|
||||
|
||||
找不到模型/link 时抛 SimDescriptionError，并提示这里和 cameras.yaml 的
|
||||
world_model 字段必须对应 —— 配错的话相机不会有 TF，而症状是
|
||||
"tf 查询报 frame does not exist"，与相机本身无关，容易查错方向。
|
||||
"""
|
||||
text = Path(world_file).read_text(encoding="utf-8", errors="replace")
|
||||
|
||||
mblock = _find_block(text, "model", model)
|
||||
if mblock is None:
|
||||
raise SimDescriptionError(
|
||||
f"{world_file} 里找不到 <model name=\"{model}\">。\n"
|
||||
f" devices/sim/gazebo/config/cameras.yaml 里若有 world_model: {model}，"
|
||||
f"就必须在 world 里真的有这个模型 —— 否则该相机没有 TF，"
|
||||
f"应用做 tf2 查询会报 frame does not exist。"
|
||||
)
|
||||
|
||||
mp = _own_pose(mblock, ("link", "model", "joint", "plugin"))
|
||||
mq = _rpy_to_quat(mp[3], mp[4], mp[5])
|
||||
|
||||
if not link:
|
||||
return ((mp[0], mp[1], mp[2]), (mp[3], mp[4], mp[5]))
|
||||
|
||||
lblock = _find_block(mblock, "link", link)
|
||||
if lblock is None:
|
||||
raise SimDescriptionError(
|
||||
f"{world_file} 的 <model name=\"{model}\"> 里找不到 <link name=\"{link}\">。"
|
||||
)
|
||||
|
||||
lp = _own_pose(lblock, ("visual", "collision", "sensor", "light"))
|
||||
lq = _rpy_to_quat(lp[3], lp[4], lp[5])
|
||||
|
||||
# 复合：世界系下的位置 = t_model + R_model * t_link
|
||||
t = _rotate(mq, (lp[0], lp[1], lp[2]))
|
||||
pos = (mp[0] + t[0], mp[1] + t[1], mp[2] + t[2])
|
||||
rpy = _quat_to_rpy(_quat_mul(mq, lq))
|
||||
return (pos, rpy)
|
||||
|
||||
|
||||
# 只要 world 里声明了任何一个 <plugin>，Gazebo 就不再加载 server.config 的默认
|
||||
# 系统列表，这三个基础系统必须由 world 自己声明，否则整个仿真静默失效。
|
||||
# 详细后果见 assembly.world 里那段注释，以及这里的 check_base_plugins()。
|
||||
BASE_SYSTEMS = {
|
||||
"Physics": "ignition::gazebo::systems::Physics",
|
||||
"UserCommands": "ignition::gazebo::systems::UserCommands",
|
||||
"SceneBroadcaster": "ignition::gazebo::systems::SceneBroadcaster",
|
||||
}
|
||||
|
||||
|
||||
def check_base_plugins(world_file: Path) -> List[str]:
|
||||
"""检查 world 是否漏声明基础系统。返回缺失的短名列表（空 = 没问题）。
|
||||
|
||||
背景（实测，本机 RoboStack/conda 的 Fortress 6.16）：
|
||||
world 里**只要声明了任何一个 <plugin>**，Gazebo 就跳过 server.config 里
|
||||
的默认系统。于是少了 UserCommands → /world/<w>/create 服务不存在 →
|
||||
ros_gz_sim create 超时后回退发 topic、没人收 → **模型从未被创建**，
|
||||
但 create 节点照样打印 "OK creation of entity"。最终表现是控制器永远
|
||||
拉不起来，而每一步日志看起来都是成功的。
|
||||
|
||||
这个检查是纯文本的、不需要 Gazebo 在跑，所以放进 --check 里，
|
||||
让"改了 world 忘了加基础系统"在启动前就被指出来。
|
||||
"""
|
||||
text = Path(world_file).read_text(encoding="utf-8", errors="replace")
|
||||
if "<plugin" not in text:
|
||||
return [] # 没声明任何 plugin → Gazebo 用默认配置，反而正常
|
||||
|
||||
missing = []
|
||||
for short, fq in BASE_SYSTEMS.items():
|
||||
# 名字里有点号，用 re.escape 免得被当通配
|
||||
if not re.search(re.escape(fq), text):
|
||||
missing.append(short)
|
||||
return missing
|
||||
|
||||
|
||||
def check_tactile_collisions(
|
||||
urdf: str, expected_topics: Optional[Sequence[str]] = None
|
||||
) -> List[str]:
|
||||
"""把展开后的 URDF 转成 SDF，核对触觉传感器的引用是否真的存在。
|
||||
|
||||
返回问题列表（空 = 没问题）。``expected_topics`` 若给出，还会核对这些话题
|
||||
在 SDF 里是否真的被声明了 —— 这是**最有价值的一项**：传感器标签一旦没进
|
||||
SDF（写错位置、被 sdformat 静默丢掉），Gazebo 里就只会表现为"话题不出现、
|
||||
日志空白"，而本检查能在启动前直接指出是哪个话题缺了。
|
||||
|
||||
已实测确认能抓到的情形：
|
||||
* <contact><collision> 引用的名字在 SDF 里不存在（sdformat 命名规则变了）
|
||||
* <contact> 里没有 <topic>（sdformat 会补一个占位值 __default_topic__，
|
||||
所以这一项不能只判"空"，要连占位值一起判 —— 这是单测逼出来的）
|
||||
* 传感器标签没有产生出任何 force_torque/contact 传感器
|
||||
* 配置里声明的话题在 SDF 里找不到
|
||||
|
||||
=========================================================================
|
||||
为什么必须有这个检查
|
||||
=========================================================================
|
||||
接触传感器的 <collision> 里写的**不是我们起的名字**，而是 sdformat 从
|
||||
URDF 转 SDF 时按规则自动生成的：
|
||||
|
||||
<fixed 关节链的父 link> + "_fixed_joint_lump__" + <本 link> + "_" + collision 名
|
||||
|
||||
本机实测（Fortress 6.16）得到
|
||||
sim_left_finger_mount_fixed_joint_lump__sim_left_finger_tip_collision
|
||||
而 URDF 的 <collision> 不能命名，所以没法给它一个稳定名字。代价是：
|
||||
**换一个 sdformat 版本、命名规则一变，传感器就永远收不到数据 —— 而
|
||||
Gazebo 不会为此报任何错**（话题在、没输出，看起来像"还没接触"）。
|
||||
|
||||
同样容易静默失效的两处，这里一并核对：
|
||||
* force_torque 传感器挂的关节是否存在于 SDF（写错关节名 = 永不发布）
|
||||
* 接触传感器的话题是否在 <contact> 里（写在 <sensor> 层会被忽略，
|
||||
gz 转而使用默认的 scoped 话题名，于是"按 /tactile/left/raw 订阅"
|
||||
永远收不到东西）
|
||||
|
||||
纯文本 + 一次 `ign sdf -p`，不需要 Gazebo 在跑，所以放进启动前的检查里。
|
||||
"""
|
||||
try:
|
||||
sdf = _urdf_to_sdf(urdf)
|
||||
except SimDescriptionError as exc:
|
||||
return [f"无法把 URDF 转成 SDF 做触觉检查：{exc}"]
|
||||
|
||||
problems: List[str] = []
|
||||
try:
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
root = ET.fromstring(sdf)
|
||||
except ET.ParseError as exc:
|
||||
return [f"SDF 解析失败，无法核对触觉传感器: {exc}"]
|
||||
|
||||
# SDF 里接触传感器的 <collision> 引用的是 collision 的**限定名**。
|
||||
# ⚠️ 不要再拼一次 link 名前缀：sdformat 为 fixed 关节融合出来的 collision
|
||||
# 名字本身就已经含 link 前缀
|
||||
# （sim_left_finger_mount_fixed_joint_lump__sim_left_finger_tip_collision），
|
||||
# 拼了反而变成一个不存在的名字 —— 于是这个检查会**误报**，
|
||||
# 而误报比不检查更糟：真出问题时没人信它。两种写法都收。
|
||||
collision_names = set()
|
||||
for link in root.iter("link"):
|
||||
lname = link.get("name", "")
|
||||
for col in link.findall("collision"):
|
||||
cname = col.get("name", "")
|
||||
if cname:
|
||||
collision_names.add(cname)
|
||||
collision_names.add(f"{lname}::{cname}")
|
||||
elif lname:
|
||||
collision_names.add(lname)
|
||||
|
||||
joint_names = {j.get("name", "") for j in root.iter("joint")}
|
||||
sdf_topics = {(t.text or "").strip() for t in root.iter("topic")}
|
||||
n_contact = 0
|
||||
n_ft = 0
|
||||
|
||||
for sensor in root.iter("sensor"):
|
||||
stype = sensor.get("type", "")
|
||||
sname = sensor.get("name", "?")
|
||||
if stype == "contact":
|
||||
n_contact += 1
|
||||
contact = sensor.find("contact")
|
||||
if contact is None:
|
||||
problems.append(f"接触传感器 {sname} 没有 <contact> 块")
|
||||
continue
|
||||
ref = (contact.findtext("collision") or "").strip()
|
||||
if not ref or ref == "__default__":
|
||||
problems.append(
|
||||
f"接触传感器 {sname} 的 <contact> 里没有可用的 <collision>"
|
||||
f"（实测值: {ref or '空'}）。\n"
|
||||
f" 留空不会「监测全部」：sdformat 会补成 __default__"
|
||||
f"（一个不存在的 collision），症状是永远没有输出。"
|
||||
)
|
||||
elif ref not in collision_names:
|
||||
problems.append(
|
||||
f"接触传感器 {sname} 引用的 collision 在 SDF 里不存在: {ref}\n"
|
||||
f" 这几乎总是 sdformat 的自动命名规则变了。"
|
||||
f"现有 collision: {sorted(collision_names)[:4]} ...\n"
|
||||
f" 用 `ign sdf -p <展开后的 urdf>` 看实际名字，"
|
||||
f"改 devices/sim/gazebo/urdf/arm.sim.sensors.xacro 里的 <collision>。"
|
||||
)
|
||||
# ⚠️ 同样要防 sdformat 的占位值：缺 <topic> 时它会补一个
|
||||
# __default_topic__（实测），于是"有没有 <topic>"这种检查会被骗过去。
|
||||
# __default_topic__ 不是可用话题名 —— gz 实际发的是
|
||||
# /world/<w>/model/<m>/link/<l>/sensor/<s>/contact。
|
||||
ctopic = (contact.findtext("topic") or "").strip()
|
||||
if ctopic in ("", "__default_topic__"):
|
||||
problems.append(
|
||||
f"接触传感器 {sname} 的 <contact> 里没有可用 <topic>"
|
||||
f"（实测值: {ctopic or '空'}）。\n"
|
||||
f" 话题必须写在 contact/topic（写在 <sensor> 层会被静默忽略，"
|
||||
f"gz 改用默认 scoped 话题名）。"
|
||||
)
|
||||
elif stype == "force_torque":
|
||||
n_ft += 1
|
||||
parent = _sdf_parent_joint(root, sensor)
|
||||
if parent is None:
|
||||
problems.append(
|
||||
f"力传感器 {sname} 不在任何 <joint> 里。\n"
|
||||
f" force_torque 在 SDF 里只能属于关节；用 "
|
||||
f"<gazebo reference=「关节名」><sensor .../></gazebo> 表达，"
|
||||
f"sdformat 会把它移进对应的 <joint>。"
|
||||
)
|
||||
elif parent not in joint_names:
|
||||
problems.append(f"力传感器 {sname} 挂在未知关节 {parent} 上")
|
||||
if (sensor.findtext("topic") or "").strip() == "":
|
||||
problems.append(
|
||||
f"力传感器 {sname} 没有 <topic>（它不会用默认话题名，"
|
||||
f"而是完全不发布）。"
|
||||
)
|
||||
|
||||
# 传感器是否被整个丢掉 —— 这一项最容易被忽略，也最难现场排查。
|
||||
if n_contact == 0 and n_ft == 0:
|
||||
problems.append(
|
||||
"SDF 里**一个触觉传感器都没有**。\n"
|
||||
" 说明 arm.sim.sensors.xacro 里的 <gazebo reference=...> 名字写错、"
|
||||
"或传感器标签写到了错误的位置 —— sdformat 会把整块**静默丢掉**：\n"
|
||||
" 转换成功、无警告、Gazebo 里话题不出现、日志空白。"
|
||||
)
|
||||
if expected_topics:
|
||||
missing = [t for t in expected_topics if t not in sdf_topics]
|
||||
if missing:
|
||||
problems.append(
|
||||
f"以下触觉话题在 SDF 里不存在: {', '.join(missing)}\n"
|
||||
f" 配置（tactile.yaml）里声明了这些话题，但描述里没有产生它们。\n"
|
||||
f" 常见原因：<gazebo reference=「关节名」> 的关节名不存在"
|
||||
f"（整个传感器被丢掉），或 contact 传感器少写了 <topic>。\n"
|
||||
f" SDF 里实际有的话题: {sorted(t for t in sdf_topics if t)}"
|
||||
)
|
||||
return problems
|
||||
|
||||
|
||||
def _sdf_parent_joint(root, sensor) -> Optional[str]:
|
||||
"""在 SDF 树里找 sensor 所属的 <joint> 名。"""
|
||||
for joint in root.iter("joint"):
|
||||
for s in joint.findall("sensor"):
|
||||
if s is sensor:
|
||||
return joint.get("name", "")
|
||||
return None
|
||||
|
||||
|
||||
def _urdf_to_sdf(urdf: str) -> str:
|
||||
"""调 `ign sdf -p` 把 URDF 文本转成 SDF 文本。
|
||||
|
||||
走命令行而不是 Python 绑定：sdformat 的 Python 绑定在这个环境里没有装，
|
||||
而 ign CLI 是仿真环境的必备件（sim_env.sh 里就有）。
|
||||
"""
|
||||
import tempfile
|
||||
|
||||
with tempfile.NamedTemporaryFile("w", suffix=".urdf", delete=False,
|
||||
encoding="utf-8") as fh:
|
||||
fh.write(urdf)
|
||||
tmp = fh.name
|
||||
try:
|
||||
r = subprocess.run(
|
||||
["ign", "sdf", "-p", tmp], capture_output=True, text=True, timeout=120
|
||||
)
|
||||
except FileNotFoundError as exc:
|
||||
raise SimDescriptionError(
|
||||
"找不到 ign 命令，无法做 SDF 转换检查。修法: source scripts/sim_env.sh"
|
||||
) from exc
|
||||
finally:
|
||||
try:
|
||||
os.unlink(tmp)
|
||||
except OSError:
|
||||
pass
|
||||
if r.returncode != 0:
|
||||
raise SimDescriptionError(
|
||||
f"ign sdf 转换失败（退出码 {r.returncode}）: {(r.stderr or r.stdout).strip()[:600]}"
|
||||
)
|
||||
return r.stdout
|
||||
|
||||
|
||||
def expand_xacro(
|
||||
xacro_path: Path,
|
||||
mappings: Optional[Dict[str, object]] = None,
|
||||
) -> str:
|
||||
"""跑 xacro 展开。
|
||||
|
||||
优先用 Python 模块（避免子进程与 PATH 依赖），失败再退回命令行。
|
||||
两者行为一致，但模块方式能拿到更干净的报错。
|
||||
"""
|
||||
xacro_path = Path(xacro_path)
|
||||
if not xacro_path.is_file():
|
||||
raise SimDescriptionError(f"xacro 文件不存在: {xacro_path}")
|
||||
|
||||
try:
|
||||
import xacro # type: ignore
|
||||
except ImportError:
|
||||
return _expand_via_cli(xacro_path, mappings)
|
||||
|
||||
try:
|
||||
doc = xacro.process_file(str(xacro_path), mappings=mappings or {})
|
||||
except Exception as exc: # xacro 的异常类型很杂，统一转成可读报错
|
||||
raise SimDescriptionError(
|
||||
f"xacro 展开失败: {xacro_path}\n 原因: {exc}\n"
|
||||
f" 单独复现: xacro {xacro_path}"
|
||||
) from exc
|
||||
return doc.toxml()
|
||||
|
||||
|
||||
def _expand_via_cli(xacro_path: Path, mappings: Optional[Dict[str, object]]) -> str:
|
||||
cmd = ["xacro", str(xacro_path)]
|
||||
for k, v in (mappings or {}).items():
|
||||
cmd.append(f"{k}:={v}")
|
||||
try:
|
||||
r = subprocess.run(cmd, capture_output=True, text=True, timeout=120)
|
||||
except FileNotFoundError as exc:
|
||||
raise SimDescriptionError(
|
||||
"既 import 不到 xacro 模块，也找不到 xacro 命令。\n"
|
||||
" 修法: source scripts/sim_env.sh"
|
||||
) from exc
|
||||
if r.returncode != 0:
|
||||
raise SimDescriptionError(
|
||||
f"xacro 展开失败（退出码 {r.returncode}）: {xacro_path}\n"
|
||||
f" {(r.stderr or r.stdout).strip()[:800]}"
|
||||
)
|
||||
return r.stdout
|
||||
|
||||
|
||||
def prepare_urdf(
|
||||
xacro_path: Optional[Path] = None,
|
||||
controllers_file: Optional[Path] = None,
|
||||
load_gripper: bool = True,
|
||||
with_sensors: bool = True,
|
||||
rizon_type: str = "Rizon4",
|
||||
arm_prefix: str = "sim",
|
||||
robot_sn: str = "",
|
||||
wrist_ft_topic: str = "",
|
||||
) -> Tuple[str, NormalizeReport]:
|
||||
"""展开 + 归一化，返回 (URDF 文本, 归一化报告)。
|
||||
|
||||
调用方**必须**把这份 URDF 同时用于两处，否则会出问题：
|
||||
- robot_state_publisher 发布到 /robot_description
|
||||
- ros_gz_sim create 用它 spawn 进 Gazebo
|
||||
gz_ros2_control 的插件是从 /robot_description 读 <ros2_control> 的
|
||||
（SDF 里只存了插件名和参数文件路径，接口声明不在 SDF 里，实测确认）。
|
||||
所以若 RSP 发的是**未归一化**的那份，控制器注册的关节名就会和 Gazebo 里的
|
||||
对不上，症状是 "Joint 'sim_joint1' not found in the robot model"。
|
||||
|
||||
:param wrist_ft_topic: 腕部 FT 的 gz 话题名。**从 wrist_ft.yaml 传进来**，
|
||||
而不是让 xacro 里再写一份 —— 话题名有三处要对上（配置、URDF、
|
||||
应用层按 sn 推出的名字），两处写死迟早漂移。空 = 用 xacro 的默认值。
|
||||
"""
|
||||
root = repo_root()
|
||||
xacro_path = Path(xacro_path) if xacro_path else default_xacro(root)
|
||||
controllers_file = (
|
||||
Path(controllers_file) if controllers_file else default_controllers(root)
|
||||
)
|
||||
|
||||
if not Path(controllers_file).is_file():
|
||||
raise SimDescriptionError(
|
||||
f"控制器配置不存在: {controllers_file}\n"
|
||||
f" 它是 arm.sim.gazebo.xacro 里系统插件的 <parameters> 指向的文件，"
|
||||
f"缺了会导致 Gazebo 报 'Failed to load system plugin'。"
|
||||
)
|
||||
|
||||
xacro_args = {
|
||||
"controllers_file": str(Path(controllers_file).resolve()),
|
||||
"load_gripper": "true" if load_gripper else "false",
|
||||
"with_sensors": "true" if with_sensors else "false",
|
||||
"rizon_type": rizon_type,
|
||||
"arm_prefix": arm_prefix,
|
||||
"robot_sn": robot_sn,
|
||||
}
|
||||
# 只在给了值时才覆盖，否则保持 xacro 里那个有注释、能单独跑 xacro 的默认值。
|
||||
if wrist_ft_topic:
|
||||
xacro_args["wrist_ft_topic"] = wrist_ft_topic
|
||||
|
||||
urdf = expand_xacro(xacro_path, xacro_args)
|
||||
normalized, report = normalize_urdf(urdf)
|
||||
return normalized, report
|
||||
+309
@@ -0,0 +1,309 @@
|
||||
"""sim_camera_adapter —— 把桥接过来的 Gazebo 相机，变成应用能直接吃的话题。
|
||||
|
||||
=============================================================================
|
||||
它到底解决什么问题
|
||||
=============================================================================
|
||||
ros_gz_bridge 只做"传输"：把 gz 的消息类型翻成 ROS 的消息类型，话题名照抄。
|
||||
它**改不了**两样东西，而这两样恰恰是仿真里最容易错、也最不报错的：
|
||||
|
||||
1. frame_id
|
||||
gz 盖的是内部路径（实测是 rizon_sim/sim_link7/d405_rgbd），
|
||||
而 TF 里真实存在的坐标系叫 sim_d405_link —— 两者对不上。
|
||||
更麻烦的是：即使改成 sim_d405_link 也还是错的，因为那是"相机机身"系
|
||||
（X 前 / Y 左 / Z 上），而 ROS 的 image/camera_info 必须是光学系
|
||||
（Z 前 / X 右 / Y 下）。tf2 查询两种都能查到、都不报错，
|
||||
但用错的那个把深度反投影成三维点会整体转错。
|
||||
所以这里盖的是 <相机>_color_optical_frame，并同时发布
|
||||
link → optical 的静态 TF。
|
||||
|
||||
2. 深度单位
|
||||
gz 是 32FC1 / 米，真机 realsense 的 aligned_depth 是 16UC1 / 毫米。
|
||||
不转换的话应用按毫米解释会整体差 1000 倍，而且 dtype 不符时
|
||||
numpy 读出来直接是垃圾。
|
||||
|
||||
=============================================================================
|
||||
为什么是"桥接 + 适配器"两层，而不是合成一层
|
||||
=============================================================================
|
||||
桥接器改不了 frame_id，所以无论如何都要有第二层做语义转换。
|
||||
既然必须有，就让第二层把"改名 + 坐标系 + 单位"全都集中处理，
|
||||
而不是让桥接器改一半、适配器补一半 —— 那样出问题时不知道去哪找。
|
||||
副产品：gz 原名的话题（/d405/image 等）在 ROS 图里仍然可见，
|
||||
排查"是桥接断了还是适配器断了"时能一刀切开。
|
||||
|
||||
=============================================================================
|
||||
发布的话题（与真机完全一致，应用无需为仿真改任何代码）
|
||||
=============================================================================
|
||||
/<ros_ns>/color/image_raw rgb8
|
||||
/<ros_ns>/color/camera_info plumb_bob（畸变全 0，gz 不模拟畸变）
|
||||
/<ros_ns>/aligned_depth_to_color/image_raw 16UC1 毫米，0 = 无读数
|
||||
|
||||
QoS 用 sensor data（BEST_EFFORT, keep_last 1），与真机 realsense 发布端、
|
||||
以及应用的订阅端（qos_profile_sensor_data）都对齐。这一点不能想当然：
|
||||
发布端 BEST_EFFORT、订阅端 RELIABLE 是不兼容的，表现是"话题在、echo 不到"。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from typing import Dict, List, Optional
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import (
|
||||
DurabilityPolicy,
|
||||
HistoryPolicy,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
from sensor_msgs.msg import CameraInfo, Image
|
||||
from tf2_ros import StaticTransformBroadcaster
|
||||
|
||||
from .sim_cameras import (
|
||||
DEFAULT_CAMERAS_REL,
|
||||
CameraConfigError,
|
||||
CameraSpec,
|
||||
depth_m_to_mm16,
|
||||
intrinsics,
|
||||
load_cameras,
|
||||
optical_rpy,
|
||||
rpy_to_quat,
|
||||
)
|
||||
|
||||
# 图像上盖的坐标系（光学系）相对相机本体系的固定旋转。
|
||||
# 见 sim_cameras.OPTICAL_RPY 的说明 —— 这是"看起来正常但三维全错"的那一项。
|
||||
|
||||
|
||||
class SimCameraAdapter(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim_camera_adapter")
|
||||
|
||||
self.declare_parameter("cameras_file", "")
|
||||
path = str(self.get_parameter("cameras_file").value or "").strip()
|
||||
|
||||
try:
|
||||
specs = load_cameras(path or None, root=self._repo_root())
|
||||
except Exception as exc: # 配置错误应当直接说清怎么修，而不是抛栈
|
||||
self.get_logger().error(f"\n仿真相机清单读取失败：\n{exc}")
|
||||
raise
|
||||
|
||||
self._specs: List[CameraSpec] = specs
|
||||
self._subs: List[object] = []
|
||||
self._pubs: Dict[str, object] = {}
|
||||
self._warned: set = set()
|
||||
|
||||
# 相机本体 → 光学系的静态 TF。必须在图像到达**之前**就发出去，
|
||||
# 因为 TF 是 transient_local（迟到的订阅者也能拿到），但若从没发过，
|
||||
# 应用查 tf 会直接报 "frame does not exist"。
|
||||
self._tf = StaticTransformBroadcaster(self)
|
||||
self._publish_optical_tfs()
|
||||
|
||||
for s in specs:
|
||||
self._wire(s)
|
||||
|
||||
names = ", ".join(s.ros_ns for s in specs)
|
||||
self.get_logger().info(
|
||||
f"仿真相机适配器就绪: {names}"
|
||||
+ f"（深度 {sum(1 for s in specs if s.depth)}/台 已做 米→毫米 转换）"
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------ 内部
|
||||
def _repo_root(self):
|
||||
"""复用 robot_description 的仓库根定位（它已经处理了三种情形）。"""
|
||||
from .robot_description import repo_root
|
||||
|
||||
try:
|
||||
return repo_root()
|
||||
except Exception:
|
||||
return None
|
||||
|
||||
def _qos(self) -> QoSProfile:
|
||||
# 深拷贝一份 sensor data QoS：共享的那个对象若被改动会影响其它节点。
|
||||
return QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=qos_profile_sensor_data.depth,
|
||||
)
|
||||
|
||||
def _publish_optical_tfs(self) -> None:
|
||||
"""发布 link → optical 的静态 TF（每台相机一条，平移为零）。"""
|
||||
msgs = []
|
||||
for s in self._specs:
|
||||
t = TransformStamped()
|
||||
t.header.stamp = self.get_clock().now().to_msg()
|
||||
t.header.frame_id = s.link_frame
|
||||
t.child_frame_id = s.optical_frame
|
||||
t.transform.translation.x = 0.0
|
||||
t.transform.translation.y = 0.0
|
||||
t.transform.translation.z = 0.0
|
||||
qx, qy, qz, qw = rpy_to_quat(optical_rpy())
|
||||
t.transform.rotation.x = qx
|
||||
t.transform.rotation.y = qy
|
||||
t.transform.rotation.z = qz
|
||||
t.transform.rotation.w = qw
|
||||
msgs.append(t)
|
||||
self._tf.sendTransform(msgs)
|
||||
|
||||
def _wire(self, s: CameraSpec) -> None:
|
||||
qos = self._qos()
|
||||
|
||||
self._pubs[s.ros_color] = self.create_publisher(Image, s.ros_color, qos)
|
||||
self._pubs[s.ros_info] = self.create_publisher(CameraInfo, s.ros_info, qos)
|
||||
if s.depth:
|
||||
self._pubs[s.ros_depth] = self.create_publisher(Image, s.ros_depth, qos)
|
||||
|
||||
self._subs.append(
|
||||
self.create_subscription(
|
||||
Image, s.gz_image, lambda m, sp=s: self._on_color(sp, m), qos
|
||||
)
|
||||
)
|
||||
self._subs.append(
|
||||
self.create_subscription(
|
||||
CameraInfo, s.gz_info, lambda m, sp=s: self._on_info(sp, m), qos
|
||||
)
|
||||
)
|
||||
if s.depth:
|
||||
self._subs.append(
|
||||
self.create_subscription(
|
||||
Image, s.gz_depth, lambda m, sp=s: self._on_depth(sp, m), qos
|
||||
)
|
||||
)
|
||||
|
||||
def _restamp(self, msg, frame_id: str) -> None:
|
||||
"""把坐标系换掉。就地改，避免每帧多一次消息构造与一次大数组拷贝。"""
|
||||
if msg.header.frame_id != frame_id:
|
||||
msg.header.frame_id = frame_id
|
||||
|
||||
def _on_color(self, s: CameraSpec, msg: Image) -> None:
|
||||
self._note_encoding(s, msg)
|
||||
self._restamp(msg, s.optical_frame)
|
||||
self._pubs[s.ros_color].publish(msg)
|
||||
|
||||
def _on_info(self, s: CameraSpec, msg: CameraInfo) -> None:
|
||||
"""重算并覆盖内参 —— gz 发的那份是错的，不能透传。
|
||||
|
||||
本机实测：gz 恒发 fx=fy=277, cx=160, cy=120（等于它的默认 60° 视场 +
|
||||
半分辨率主点），而渲染用的是配置的 87° 视场与 640x480。
|
||||
照抄它做三维反投影会错半米量级，且单目预览里完全看不出来。
|
||||
详见 sim_cameras.intrinsics() 的说明。
|
||||
"""
|
||||
w = int(msg.width) or int(s.width)
|
||||
h = int(msg.height) or int(s.height)
|
||||
try:
|
||||
k, p = intrinsics(s, w, h)
|
||||
except CameraConfigError as exc:
|
||||
key = ("info", s.ros_ns)
|
||||
if key not in self._warned:
|
||||
self._warned.add(key)
|
||||
self.get_logger().error(f"{s.ros_ns}: 内参算不出来，{s.ros_info} 无输出。{exc}")
|
||||
return
|
||||
|
||||
key = ("info", s.ros_ns)
|
||||
if key not in self._warned:
|
||||
self._warned.add(key)
|
||||
gz_k = list(msg.k) if len(msg.k) == 9 else []
|
||||
self.get_logger().info(
|
||||
f"{s.ros_ns}: 内参已按视场 {s.horizontal_fov:.4f} rad "
|
||||
f"({math.degrees(s.horizontal_fov):.1f}°) 与 {w}x{h} 重算 —— "
|
||||
f"fx={k[0]:.1f} cx={k[2]:.1f} cy={k[5]:.1f}"
|
||||
+ (
|
||||
f"（覆盖 gz 的 fx={gz_k[0]:.1f} cx={gz_k[2]:.1f} cy={gz_k[5]:.1f}）"
|
||||
if gz_k
|
||||
else ""
|
||||
)
|
||||
)
|
||||
|
||||
out = CameraInfo()
|
||||
out.header = msg.header
|
||||
out.header.frame_id = s.optical_frame
|
||||
out.height = h
|
||||
out.width = w
|
||||
out.distortion_model = "plumb_bob"
|
||||
# gz 不模拟镜头畸变，所以畸变系数全 0、R 为单位阵。
|
||||
# 这意味着"仿真里用畸变矫正调出来的参数"在真机上仍需重调 —— 已在
|
||||
# devices/sim/gazebo/README.md 里说明。
|
||||
out.d = [0.0] * 5
|
||||
out.k = list(k)
|
||||
out.r = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
|
||||
out.p = list(p)
|
||||
self._pubs[s.ros_info].publish(out)
|
||||
|
||||
def _on_depth(self, s: CameraSpec, msg: Image) -> None:
|
||||
"""32FC1 米 → 16UC1 毫米。
|
||||
|
||||
用 frombuffer 而不是 array(...)：后者会把整个 buffer 再拷一遍。
|
||||
这一步每帧都在跑（30Hz × 640×480×4B），能省则省。
|
||||
"""
|
||||
if msg.encoding != "32FC1":
|
||||
# 只提醒一次，避免刷屏
|
||||
key = (s.ros_ns, msg.encoding)
|
||||
if key not in self._warned:
|
||||
self._warned.add(key)
|
||||
self.get_logger().warn(
|
||||
f"{s.gz_depth} 的编码是 {msg.encoding}，不是预期的 32FC1。"
|
||||
f"已跳过本次转换 —— 深度话题会没有输出。"
|
||||
f"若 Gazebo 版本改动了深度编码，需要同步改 sim_cameras."
|
||||
f"depth_m_to_mm16 的调用处。"
|
||||
)
|
||||
return
|
||||
|
||||
arr = np.frombuffer(msg.data, dtype=np.float32).reshape(msg.height, msg.width)
|
||||
mm = depth_m_to_mm16(arr)
|
||||
|
||||
out = Image()
|
||||
out.header = msg.header
|
||||
out.header.frame_id = s.optical_frame
|
||||
out.height = msg.height
|
||||
out.width = msg.width
|
||||
out.encoding = "16UC1"
|
||||
# gz 给的是连续 buffer；转成 uint16 后每像素 2 字节，step 必须同步改，
|
||||
# 否则下游按 RGB 的 stride 解读会错位。
|
||||
out.is_bigendian = 0
|
||||
out.step = msg.width * 2
|
||||
out.data = mm.tobytes()
|
||||
self._pubs[s.ros_depth].publish(out)
|
||||
|
||||
def _note_encoding(self, s: CameraSpec, msg: Image) -> None:
|
||||
"""第一次收到彩色帧时把编码打出来。
|
||||
|
||||
为什么值得单独打印：gz 给的是 rgb8（实测），而真机 realsense 的
|
||||
color/image_raw 也是 rgb8 —— 一致，所以不需要转换。但这一点是实测
|
||||
结论，不是保证；换个 Gazebo 版本或传感器配置可能变成 bgr8。
|
||||
打一行日志，使"颜色不对"时能立刻定位到是不是编码变了。
|
||||
"""
|
||||
key = ("enc", s.ros_ns, msg.encoding)
|
||||
if key not in self._warned:
|
||||
self._warned.add(key)
|
||||
self.get_logger().info(
|
||||
f"{s.ros_ns}: 彩色 {msg.width}x{msg.height} encoding={msg.encoding} "
|
||||
f"→ {s.ros_color} (frame={s.optical_frame})"
|
||||
)
|
||||
if msg.encoding not in ("rgb8", "bgr8"):
|
||||
self.get_logger().warn(
|
||||
f"{s.ros_ns} 的彩色编码是 {msg.encoding}，既不是 rgb8 也不是 "
|
||||
f"bgr8。应用按 rgb8 解释会得到错误的颜色。"
|
||||
)
|
||||
|
||||
|
||||
def main(argv: Optional[List[str]] = None) -> int:
|
||||
rclpy.init(args=argv)
|
||||
try:
|
||||
node = SimCameraAdapter()
|
||||
except Exception:
|
||||
# 参数/配置错误时 rclpy 已经打印了原因；这里只需保证退出码非零，
|
||||
# 让 launch 能把它当成失败（否则会留下一个没有相机的"成功"启动）。
|
||||
rclpy.try_shutdown()
|
||||
return 2
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,532 @@
|
||||
"""仿真相机的配置解析与格式换算。
|
||||
|
||||
=============================================================================
|
||||
为什么单独一个模块
|
||||
=============================================================================
|
||||
"把 Gazebo 的相机输出变成应用能直接吃的话题"这件事里，真正有内容的是三件事：
|
||||
|
||||
1. 深度单位换算 32FC1 米 → 16UC1 毫米（且 0 表示无读数）
|
||||
2. 坐标系盖章 图像要盖**光学系**，不能盖相机本体系
|
||||
3. 话题改名 /d405/image → /d405/color/image_raw 等
|
||||
|
||||
这三件事全是纯数据变换，不需要 ROS 跑起来就能验。所以实现放在这里、
|
||||
用普通函数表达，节点（sim_camera_adapter.py）只做订阅/发布这层壳。
|
||||
好处是换算逻辑可以被单测钉死 —— 深度单位差 1000 倍这种错误，
|
||||
在单目预览里肉眼看不出任何异常，只能靠测试兜住。
|
||||
|
||||
另外，启动文件里生成 ros_gz_bridge 参数也要用同一份相机清单，
|
||||
所以清单的解析也放在这里，避免"桥接用的清单"和"适配器用的清单"各写一份。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import re
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
import yaml
|
||||
|
||||
# 默认配置位置（相对仓库根）。与其它 devices/ 下的配置一样，是**物料**而非代码。
|
||||
DEFAULT_CAMERAS_REL = "devices/sim/gazebo/config/cameras.yaml"
|
||||
|
||||
# 相机本体系（X 前 / Y 左 / Z 上）→ 光学系（Z 前 / X 右 / Y 下）的固定旋转。
|
||||
# 这就是真机 realsense 的 <相机>_color_optical_frame 相对其 link 用的一组角，
|
||||
# 且是 ROS 的惯例（REP-103 / REP-105）。
|
||||
#
|
||||
# ⚠️ 这一步很容易被省掉，而且省掉之后**什么都不会报错**：
|
||||
# 应用拿 image 的 frame_id 去做 tf2 查询，本体系是存在的，查询成功；
|
||||
# 但那是"相机机身"的朝向，用它把深度反投影成三维点会整体转错。
|
||||
# 单目预览看起来完全正常，只有把点投回世界坐标时才露馅 —— 所以
|
||||
# 专门有一个端到端测试把"深度反投影后应落在已知平面"作为断言。
|
||||
OPTICAL_RPY: Tuple[float, float, float] = (-1.5708, 0.0, -1.5708)
|
||||
|
||||
# uint16 能表示的最大毫米数（65535 mm = 65.535 m）。
|
||||
# 更远的读数按"无读数"处理（0），而不是饱和到 65535 ——
|
||||
# 饱和会让远处凭空出现一个 65.5m 的平面，比"无效"更有害。
|
||||
MAX_MM = 65535
|
||||
|
||||
|
||||
class CameraConfigError(RuntimeError):
|
||||
"""配置有问题时抛出，消息面向使用者。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class CameraSpec:
|
||||
"""一台仿真相机的声明。"""
|
||||
|
||||
gz_ns: str
|
||||
ros_ns: str
|
||||
link_frame: str
|
||||
optical_frame: str
|
||||
depth: bool = True
|
||||
horizontal_fov: float = 0.0
|
||||
world_model: str = ""
|
||||
world_link: str = "link"
|
||||
width: int = 0
|
||||
height: int = 0
|
||||
|
||||
# --- Gazebo 侧（ros_gz_bridge 的输入）-----------------------------------
|
||||
@property
|
||||
def gz_image(self) -> str:
|
||||
return f"{self.gz_ns}/image"
|
||||
|
||||
@property
|
||||
def gz_depth(self) -> str:
|
||||
return f"{self.gz_ns}/depth_image"
|
||||
|
||||
@property
|
||||
def gz_info(self) -> str:
|
||||
return f"{self.gz_ns}/camera_info"
|
||||
|
||||
# --- ROS 侧（应用按这些名字订阅，与真机 realsense 一致）------------------
|
||||
@property
|
||||
def ros_color(self) -> str:
|
||||
return f"/{self.ros_ns}/color/image_raw"
|
||||
|
||||
@property
|
||||
def ros_info(self) -> str:
|
||||
return f"/{self.ros_ns}/color/camera_info"
|
||||
|
||||
@property
|
||||
def ros_depth(self) -> str:
|
||||
return f"/{self.ros_ns}/aligned_depth_to_color/image_raw"
|
||||
|
||||
@property
|
||||
def is_world_mounted(self) -> bool:
|
||||
"""挂在世界模型上（而非机器人本体）→ 需要额外补一条静态 TF。"""
|
||||
return bool(self.world_model)
|
||||
|
||||
|
||||
def load_cameras(path: Optional[Path] = None, root: Optional[Path] = None) -> List[CameraSpec]:
|
||||
"""读相机清单。找不到或写错时抛 CameraConfigError，消息里给出修法。"""
|
||||
if path is None:
|
||||
base = root if root is not None else Path.cwd()
|
||||
path = Path(base) / DEFAULT_CAMERAS_REL
|
||||
path = Path(path)
|
||||
|
||||
if not path.is_file():
|
||||
raise CameraConfigError(
|
||||
f"相机清单不存在: {path}\n"
|
||||
f" 它是仿真相机的话题/坐标系唯一来源（桥接与适配器都从它派生）。\n"
|
||||
f" 缺了它仿真里的相机就完全没输出。确认仓库根下存在 "
|
||||
f"{DEFAULT_CAMERAS_REL}。"
|
||||
)
|
||||
|
||||
try:
|
||||
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except yaml.YAMLError as exc:
|
||||
raise CameraConfigError(f"相机清单 YAML 解析失败: {path}\n 原因: {exc}") from exc
|
||||
|
||||
raw = doc.get("cameras")
|
||||
if not isinstance(raw, list) or not raw:
|
||||
raise CameraConfigError(
|
||||
f"{path} 里没有 cameras 列表（或为空）。至少写一台相机，否则仿真里没有图像。"
|
||||
)
|
||||
|
||||
specs: List[CameraSpec] = []
|
||||
seen: Dict[str, str] = {}
|
||||
for i, item in enumerate(raw):
|
||||
if not isinstance(item, dict):
|
||||
raise CameraConfigError(f"{path} 的 cameras[{i}] 不是映射（键值对）。")
|
||||
|
||||
def need(key: str) -> str:
|
||||
v = str(item.get(key, "") or "").strip()
|
||||
if not v:
|
||||
raise CameraConfigError(
|
||||
f"{path} 的 cameras[{i}] 缺少 {key}。"
|
||||
f"（gz_ns / ros_ns / link_frame / optical_frame 都必填）"
|
||||
)
|
||||
return v
|
||||
|
||||
# 必填键先查完，再查取值是否合理 —— 否则一份"缺键且键值也不对"的配置
|
||||
# 会先报一个后者的错，让人以为只是值填错，实际键都还没写全。
|
||||
gz_ns = "/" + need("gz_ns").strip("/")
|
||||
ros_ns = need("ros_ns").strip("/")
|
||||
link_frame = need("link_frame")
|
||||
optical_frame = need("optical_frame")
|
||||
|
||||
# 同一个 ROS 命名空间出现两次 → 两台相机会互相覆盖话题，
|
||||
# 表现为"只有一台出图"或图像在两个相机之间跳，很难查。直接拒绝。
|
||||
if ros_ns in seen:
|
||||
raise CameraConfigError(
|
||||
f"{path}: ros_ns '{ros_ns}' 重复（cameras[{i}] 与 {seen[ros_ns]}）。\n"
|
||||
f" 两台相机用同一个 ros_ns 会互相覆盖话题，必须区分开。"
|
||||
)
|
||||
seen[ros_ns] = f"cameras[{i}]"
|
||||
|
||||
try:
|
||||
fov = float(item.get("horizontal_fov", 0.0) or 0.0)
|
||||
except (TypeError, ValueError):
|
||||
raise CameraConfigError(
|
||||
f"{path} 的 cameras[{i}].horizontal_fov 不是数字。"
|
||||
) from None
|
||||
# 视场是内参的唯一来源（gz 自己发的 camera_info 是错的，见文件头），
|
||||
# 所以它必须存在且合理。填错的话三维点位会整体错，而且看不出来。
|
||||
if not (0.0 < fov < math.pi):
|
||||
raise CameraConfigError(
|
||||
f"{path} 的 cameras[{i}].horizontal_fov = {fov} 不合理。\n"
|
||||
f" 必须是 (0, π) 弧度之间的值，例如真机 D405 约 87° = 1.5184。\n"
|
||||
f" 它决定内参 fx = fy = W/(2*tan(hfov/2))，填错会让反投影出的"
|
||||
f"三维点整体偏移，而单目预览里完全看不出来。"
|
||||
)
|
||||
|
||||
specs.append(
|
||||
CameraSpec(
|
||||
gz_ns=gz_ns,
|
||||
ros_ns=ros_ns,
|
||||
link_frame=link_frame,
|
||||
optical_frame=optical_frame,
|
||||
depth=bool(item.get("depth", True)),
|
||||
horizontal_fov=fov,
|
||||
world_model=str(item.get("world_model", "") or "").strip(),
|
||||
world_link=str(item.get("world_link", "") or "link").strip() or "link",
|
||||
width=int(item.get("width", 0) or 0),
|
||||
height=int(item.get("height", 0) or 0),
|
||||
)
|
||||
)
|
||||
return specs
|
||||
|
||||
|
||||
def intrinsics(spec: CameraSpec, width: int, height: int) -> Tuple[List[float], List[float]]:
|
||||
"""由视场与图像尺寸算出 (K, P)。返回扁平列表，可直接塞进 CameraInfo。
|
||||
|
||||
=========================================================================
|
||||
为什么必须自己算，而不是用 gz 发的那份 camera_info
|
||||
=========================================================================
|
||||
本机实测（Fortress 6.16 / RoboStack）：gz 给 rgbd_camera 发的 camera_info
|
||||
**恒为** fx=fy=277, cx=160, cy=120 —— 无论 SDF 里怎么配。这组值对应 gz 的
|
||||
默认参数：60° 视场 + 320x240 图幅。
|
||||
|
||||
而它**渲染**用的却是 SDF 里配的 87° 视场与 640x480。证据是：用已知地面平面
|
||||
对深度反投影做最小二乘，解出 fy=337.2、cy=239.5，与
|
||||
640 / (2*tan(1.5184/2)) = 337.2 480/2 = 240
|
||||
完全吻合（残差中位 0.2mm）。也就是说渲染是对的，只有 camera_info 是默认值。
|
||||
|
||||
后果有多严重：照抄那份 camera_info 反投影，D435 看到的地面会被算到
|
||||
z ≈ -0.49m（真实是 0），偏差半米量级。而单目预览一切正常 ——
|
||||
这类错误只有在用三维点位时才暴露，恰好是本项目装配任务的核心输入。
|
||||
|
||||
因此这里按"渲染实际用的视场"重算，并覆盖 gz 的 camera_info。
|
||||
像素是方的（gz 由同一视场与宽高比推出两个方向），所以 fx = fy。
|
||||
|
||||
⚠️ 相应地，horizontal_fov 必须与 SDF 里真正渲染用的值一致。
|
||||
它有两个来源，启动时会自动比对并告警（check_fov_consistency）。
|
||||
"""
|
||||
if not (0.0 < spec.horizontal_fov < math.pi):
|
||||
raise CameraConfigError(
|
||||
f"相机 {spec.ros_ns} 的 horizontal_fov={spec.horizontal_fov} 不合理，"
|
||||
f"无法算内参。"
|
||||
)
|
||||
if width <= 0 or height <= 0:
|
||||
raise CameraConfigError(
|
||||
f"相机 {spec.ros_ns} 的图像尺寸非法（{width}x{height}），无法算内参。"
|
||||
)
|
||||
|
||||
fx = width / (2.0 * math.tan(spec.horizontal_fov / 2.0))
|
||||
fy = fx # 方形像素
|
||||
cx = width / 2.0
|
||||
cy = height / 2.0
|
||||
|
||||
k = [fx, 0.0, cx,
|
||||
0.0, fy, cy,
|
||||
0.0, 0.0, 1.0]
|
||||
# P 是 3x4 的投影矩阵。gz 不模拟畸变，所以 R 是单位阵、最后一列为 0。
|
||||
p = [fx, 0.0, cx, 0.0,
|
||||
0.0, fy, cy, 0.0,
|
||||
0.0, 0.0, 1.0, 0.0]
|
||||
return k, p
|
||||
|
||||
|
||||
def check_camera_consistency(
|
||||
specs: Sequence[CameraSpec],
|
||||
world_file: Optional[Path] = None,
|
||||
sensors_xacro: Optional[Path] = None,
|
||||
) -> List[str]:
|
||||
"""比对 cameras.yaml 的声明与 SDF 里真正渲染用的值（视场 + 图幅）。
|
||||
|
||||
为什么值得做这个检查：内参由 cameras.yaml 的 horizontal_fov 与 width/height
|
||||
算出，而**渲染**用的是 SDF 里的值。两者一旦不一致，图像与内参就对不上了 ——
|
||||
症状仍然是"三维点整体错"，而单目预览看不出来。这正是这个模块开头
|
||||
那一长串说明所警告的那类错误，所以要主动比对。
|
||||
|
||||
⚠️ 图幅（width/height）为什么必须一起比：它决定 fx = W/(2·tan(hfov/2))，
|
||||
也就是"每毫米多少像素"。阶段二的孔检测在原始分辨率图上跑 HoughCircles，
|
||||
hole_detect.yaml 的像素门槛（hole_distance_min 等）只在图幅与真机相同时
|
||||
才有意义。图幅写错不会报任何错，只会让孔检测"莫名其妙不认孔" ——
|
||||
比视场写错更难查（视场错还会让点位整体偏，图幅错只是尺度不对）。
|
||||
|
||||
返回人类可读的告警列表（空 = 没问题）。
|
||||
|
||||
精度说明（有意为之，不做过度解析）：
|
||||
- 世界相机：按 <sensor> 里的 <topic> 精确匹配到对应相机，读到的是精确值。
|
||||
- 腕上相机：它在 xacro 宏里、值由参数传入，展开前无法可靠地归属到具体
|
||||
相机。所以视场退化成"文件里出现过这个值就算过"，图幅则读宏的
|
||||
**参数默认值**（image_width:=/image_height:=，launch 不会覆盖它）——
|
||||
偏宽松，但足以抓住"改了 SDF 忘了改 cameras.yaml"这种最常见的漂移。
|
||||
"""
|
||||
warnings: List[str] = []
|
||||
|
||||
if world_file is not None and Path(world_file).is_file():
|
||||
text = Path(world_file).read_text(encoding="utf-8", errors="replace")
|
||||
for s in specs:
|
||||
if not s.is_world_mounted:
|
||||
continue
|
||||
# 找到 <topic> 等于本相机 gz_ns 的那个 <sensor> 块
|
||||
fov = None
|
||||
size = (None, None)
|
||||
for m in re.finditer(r"<sensor\b[^>]*>(.*?)</sensor>", text, re.S):
|
||||
body = m.group(1)
|
||||
tm = re.search(r"<topic>\s*([^<\s]+)\s*</topic>", body)
|
||||
if not tm or "/" + tm.group(1).strip().lstrip("/") != s.gz_ns:
|
||||
continue
|
||||
fm = re.search(r"<horizontal_fov>\s*([0-9.eE+-]+)\s*</horizontal_fov>", body)
|
||||
if fm:
|
||||
fov = float(fm.group(1))
|
||||
wm = re.search(r"<width>\s*(\d+)\s*</width>", body)
|
||||
hm = re.search(r"<height>\s*(\d+)\s*</height>", body)
|
||||
size = (
|
||||
int(wm.group(1)) if wm else None,
|
||||
int(hm.group(1)) if hm else None,
|
||||
)
|
||||
break
|
||||
if fov is None:
|
||||
warnings.append(
|
||||
f"相机 {s.ros_ns}: 在 {Path(world_file).name} 里找不到 topic 为 "
|
||||
f"{s.gz_ns} 的 <sensor> 的 <horizontal_fov>，无法比对。"
|
||||
f"内参将按 cameras.yaml 的 {s.horizontal_fov} 计算 —— "
|
||||
f"若与渲染实际用的视场不符，三维点位会整体错。"
|
||||
)
|
||||
elif abs(fov - s.horizontal_fov) > 1e-4:
|
||||
warnings.append(
|
||||
f"相机 {s.ros_ns}: 视场不一致！cameras.yaml 写 {s.horizontal_fov}，"
|
||||
f"而 {Path(world_file).name} 里渲染用的是 {fov}。\n"
|
||||
f" 内参按前者算、图像按后者渲染 → 三维点位会整体错，"
|
||||
f"而单目预览看不出来。请把两处改成一致。"
|
||||
)
|
||||
_check_size(s, size[0], size[1], Path(world_file).name, warnings)
|
||||
|
||||
if sensors_xacro is not None and Path(sensors_xacro).is_file():
|
||||
text = Path(sensors_xacro).read_text(encoding="utf-8", errors="replace")
|
||||
found = [float(v) for v in re.findall(r"<horizontal_fov>\s*([0-9.eE+-]+)", text)]
|
||||
for s in specs:
|
||||
if s.is_world_mounted:
|
||||
continue
|
||||
if not found:
|
||||
warnings.append(
|
||||
f"相机 {s.ros_ns}: 在 {Path(sensors_xacro).name} 里没找到 "
|
||||
f"<horizontal_fov>，无法比对（内参按 cameras.yaml 的 "
|
||||
f"{s.horizontal_fov} 计算）。"
|
||||
)
|
||||
elif not any(abs(v - s.horizontal_fov) <= 1e-4 for v in found):
|
||||
warnings.append(
|
||||
f"相机 {s.ros_ns}: 视场可能不一致。cameras.yaml 写 "
|
||||
f"{s.horizontal_fov}，而 {Path(sensors_xacro).name} 里出现的值是 "
|
||||
f"{found}。若渲染用的不是 {s.horizontal_fov}，三维点位会整体错。"
|
||||
)
|
||||
# 腕上相机的图幅：读宏的参数默认值（launch 不覆盖它们）
|
||||
mw = re.search(r"image_width:=(\d+)", text)
|
||||
mh = re.search(r"image_height:=(\d+)", text)
|
||||
for s in specs:
|
||||
if s.is_world_mounted:
|
||||
continue
|
||||
_check_size(
|
||||
s,
|
||||
int(mw.group(1)) if mw else None,
|
||||
int(mh.group(1)) if mh else None,
|
||||
Path(sensors_xacro).name,
|
||||
warnings,
|
||||
)
|
||||
|
||||
return warnings
|
||||
|
||||
|
||||
def _check_size(
|
||||
spec: "CameraSpec",
|
||||
width: Optional[int],
|
||||
height: Optional[int],
|
||||
where: str,
|
||||
warnings: List[str],
|
||||
) -> None:
|
||||
"""比对声明图幅与实际渲染图幅，不一致就追加一条告警。
|
||||
|
||||
单独抽出来是因为世界相机与腕上相机的"值从哪读"完全不同（前者读 <sensor>，
|
||||
后者读 xacro 宏默认值），但**比对逻辑与文案必须一致** —— 两处各写一遍
|
||||
迟早会漂移成两套说法。
|
||||
|
||||
``spec`` 未声明图幅（width/height <= 0）时直接跳过：那表示"这份清单不管尺寸"，
|
||||
此时 intrinsics() 会退回 640x480。没有声明就没有"不一致"可言，
|
||||
报出来只会变成噪声，反而让人忽略真正的告警。
|
||||
"""
|
||||
if spec.width <= 0 or spec.height <= 0:
|
||||
return
|
||||
if width is None or height is None:
|
||||
warnings.append(
|
||||
f"相机 {spec.ros_ns}: 在 {where} 里读不到图像尺寸（<width>/<height> 或 "
|
||||
f"image_width:=），无法比对。cameras.yaml 声明 "
|
||||
f"{spec.width}x{spec.height} —— 若渲染的实际尺寸与它不同，"
|
||||
f"内参算出的每毫米像素数就是错的，孔检测的像素门槛会静默失效。"
|
||||
)
|
||||
return
|
||||
if (width, height) != (spec.width, spec.height):
|
||||
warnings.append(
|
||||
f"相机 {spec.ros_ns}: 图像尺寸不一致！cameras.yaml 写 "
|
||||
f"{spec.width}x{spec.height}，而 {where} 里渲染用的是 {width}x{height}。\n"
|
||||
f" 内参按前者算、图像按后者渲染 → fx 的尺度不对（差 "
|
||||
f"{width / max(spec.width, 1):.3f} 倍），"
|
||||
f"所有以像素为单位的门槛（如 hole_distance_min）都会静默失效。\n"
|
||||
f" 注意：真机 D405 是 848x480、D435 是 1280x720；仿真侧要对齐 D405。"
|
||||
)
|
||||
|
||||
|
||||
def bridge_args(specs: Sequence[CameraSpec]) -> List[str]:
|
||||
"""生成 ros_gz_bridge parameter_bridge 的话题规则。
|
||||
|
||||
只做**传输**：gz 消息类型 → ROS 消息类型，话题名保持 gz 侧的原样。
|
||||
改名与格式换算交给适配器 —— 分开的原因是桥接器改不了 frame_id，
|
||||
而 frame_id 恰恰是最容易错又最不报错的那一项。让适配器统一处理
|
||||
"名字 + 坐标系 + 单位"，逻辑集中在一处。
|
||||
|
||||
注：/d405/image 这类"gz 原名"话题在 ROS 图里也会可见。
|
||||
这是有意保留的：排查"到底是桥接断了还是适配器断了"时，
|
||||
直接 echo 原名话题能一刀切开。
|
||||
"""
|
||||
args: List[str] = []
|
||||
for s in specs:
|
||||
args.append(f"{s.gz_image}@sensor_msgs/msg/Image[gz.msgs.Image")
|
||||
args.append(f"{s.gz_info}@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo")
|
||||
if s.depth:
|
||||
# gz 的深度是 float32（本机实测 4 字节/像素），桥接器支持为 32FC1。
|
||||
# 若哪天桥接器不再支持，这里会静默少一个话题 —— 验收时用
|
||||
# `ros2 topic list | grep aligned_depth` 确认它在。
|
||||
args.append(f"{s.gz_depth}@sensor_msgs/msg/Image[gz.msgs.Image")
|
||||
return args
|
||||
|
||||
|
||||
def depth_m_to_mm16(depth_m: np.ndarray, max_mm: int = MAX_MM) -> np.ndarray:
|
||||
"""深度：32FC1 米 → 16UC1 毫米。0 表示"无读数"（与真机 realsense 一致）。
|
||||
|
||||
规则（每一条都对应一种真机会出现的情况）：
|
||||
非有限值（nan / inf） → 0。gz 在没命中时可能给 inf，
|
||||
真机 realsense 给 0；统一成 0。
|
||||
<= 0 → 0。真机对无效像素就是给 0，
|
||||
负深度没有物理意义。
|
||||
0 < d <= max_mm/1000 → round(d*1000)
|
||||
> max_mm/1000（65.535m） → 0（见 MAX_MM 的说明：不饱和）
|
||||
舍入后为 0 的极小正数 → 0（例如 0.0001m，落不进 uint16）
|
||||
|
||||
返回与输入同形状的 uint16 数组。
|
||||
"""
|
||||
a = np.asarray(depth_m)
|
||||
if a.dtype != np.float32:
|
||||
a = a.astype(np.float32, copy=False)
|
||||
|
||||
out = np.zeros(a.shape, dtype=np.uint16)
|
||||
|
||||
finite = np.isfinite(a)
|
||||
if not finite.any():
|
||||
return out
|
||||
|
||||
# 只对有限且为正的像素做换算；先把它们挑出来，避免对 nan 做比较
|
||||
# （nan 参与的比较全是 False，但显式挑出来意图更清楚，也少一遍遍历）
|
||||
cand = finite & (a > 0.0)
|
||||
if not cand.any():
|
||||
return out
|
||||
|
||||
mm = np.rint(a[cand].astype(np.float64) * 1000.0)
|
||||
ok = (mm > 0.0) & (mm <= float(max_mm))
|
||||
if ok.any():
|
||||
out[cand] = np.where(ok, mm, 0.0).astype(np.uint16)
|
||||
return out
|
||||
|
||||
|
||||
#: 腕部相机的视轴与法兰轴线的允许夹角 [度]。
|
||||
#: 真机装支架时也不会完全正装，但"看得到工件"要求它基本沿法兰轴；
|
||||
#: 20° 是给手工装配留的余量，同时足以把"少转 90°"这类错误拦下来。
|
||||
MOUNT_TOL_DEG = 20.0
|
||||
|
||||
#: 手外相机的视轴在世界系里的 z 分量上限（-1 = 正朝下）。松于 -0.5 就算没在朝下看。
|
||||
WORLD_LOOK_DOWN_COS = -0.5
|
||||
|
||||
|
||||
def flange_frame_for(spec: CameraSpec) -> Optional[str]:
|
||||
"""从相机的 link_frame 反推它所在臂的法兰 frame：sim_d405_link → sim_flange。
|
||||
|
||||
只对**挂在臂上**的相机有意义（``world_model`` 非空的是世界相机，返回 None）。
|
||||
为什么要反推而不是读配置：``site.yaml`` 的 ``sim.joint_prefix`` 自己都写着
|
||||
"✘ 未接线"，拿一个没接线的值当路径判据会静默失效。
|
||||
link_frame 里的前缀才是真正生效的那个（由 xacro 的 arm_prefix 生成）。
|
||||
"""
|
||||
if spec.world_model:
|
||||
return None
|
||||
model = spec.ros_ns.rsplit("_", 1)[-1] # sim_d405 → d405
|
||||
suffix = f"{model}_link"
|
||||
if not spec.link_frame.endswith(suffix):
|
||||
return None
|
||||
return spec.link_frame[: -len(suffix)] + "flange"
|
||||
|
||||
|
||||
def mount_axis_deg(R_flange_optical: np.ndarray) -> float:
|
||||
"""光学系视轴（+Z）与**法兰 +Z**的夹角 [度]。0 = 正装在法兰轴线上。
|
||||
|
||||
这是个可以**脱离姿态**核对的量，所以特别有用：不管机械臂当时摆成什么样，
|
||||
"相机有没有沿法兰轴线装"都只在安装处决定。
|
||||
|
||||
为什么它必须接近 0（而不是"调多少都行"）：Gz 的相机视轴是模型/连杆的 +X，
|
||||
而 OPTICAL_RPY(-90°,0,-90°) 把"机身系（X 前/Y 左/Z 上）"转成光学系。
|
||||
两者串起来 ⇒ 相机沿法兰轴线装 ⇔ 光学 +Z ∥ 法兰 +Z。
|
||||
少转那 90° 时：话题有数据、预览有图像、内参也对，只是整个视野搬到了侧面，
|
||||
应用侧表现成"目标永远不在相机前方"（曾真实踩过，见 sensors.xacro 的说明）。
|
||||
"""
|
||||
v = np.asarray(R_flange_optical, dtype=float) @ np.array([0.0, 0.0, 1.0])
|
||||
return float(np.degrees(np.arccos(float(np.clip(v[2], -1.0, 1.0)))))
|
||||
|
||||
|
||||
def world_look_down_cos(R_world_optical: np.ndarray) -> float:
|
||||
"""光学系视轴在世界系里的 z 分量（-1 = 正朝下看，0 = 平视）。"""
|
||||
return float((np.asarray(R_world_optical, dtype=float) @ np.array([0.0, 0.0, 1.0]))[2])
|
||||
|
||||
|
||||
def rpy_to_R(rpy: Sequence[float]) -> np.ndarray:
|
||||
"""URDF/SDF 的 (roll, pitch, yaw) → 3x3 旋转矩阵。
|
||||
|
||||
复合顺序是 ``R = Rz(yaw)·Ry(pitch)·Rx(roll)``（URDF 规范）—— 顺序不能想当然，
|
||||
反了在"只有一个角非零"时看不出来，两个角同时非零就会错得莫名其妙。
|
||||
"""
|
||||
r, p, y = (float(v) for v in rpy)
|
||||
cr, sr = math.cos(r), math.sin(r)
|
||||
cp, sp = math.cos(p), math.sin(p)
|
||||
cy, sy = math.cos(y), math.sin(y)
|
||||
Rx = np.array([[1.0, 0.0, 0.0], [0.0, cr, -sr], [0.0, sr, cr]])
|
||||
Ry = np.array([[cp, 0.0, sp], [0.0, 1.0, 0.0], [-sp, 0.0, cp]])
|
||||
Rz = np.array([[cy, -sy, 0.0], [sy, cy, 0.0], [0.0, 0.0, 1.0]])
|
||||
return Rz @ Ry @ Rx
|
||||
|
||||
|
||||
def mount_rpy_axis_deg(rpy: Sequence[float]) -> float:
|
||||
"""腕相机的**安装 rpy**（URDF 里那个 fixed joint 的 rpy）→ 视轴与法兰 +Z 的夹角。
|
||||
|
||||
与运行期查 TF 的 :func:`mount_axis_deg` 是同一件事，只是输入不同：
|
||||
没接 TF 时（比如核对 xacro 里的默认值）用这个算。
|
||||
"""
|
||||
return mount_axis_deg(rpy_to_R(rpy) @ rpy_to_R(OPTICAL_RPY))
|
||||
|
||||
|
||||
def optical_rpy() -> Tuple[float, float, float]:
|
||||
"""本体 → 光学系的固定旋转，供静态 TF 使用。"""
|
||||
return OPTICAL_RPY
|
||||
|
||||
|
||||
def rpy_to_quat(rpy: Sequence[float]) -> Tuple[float, float, float, float]:
|
||||
"""(roll, pitch, yaw) → (x, y, z, w)。只用于发布静态 TF。"""
|
||||
r, p, y = (float(v) for v in rpy)
|
||||
cr, sr = math.cos(r / 2.0), math.sin(r / 2.0)
|
||||
cp, sp = math.cos(p / 2.0), math.sin(p / 2.0)
|
||||
cy, sy = math.cos(y / 2.0), math.sin(y / 2.0)
|
||||
return (
|
||||
sr * cp * cy - cr * sp * sy,
|
||||
cr * sp * cy + sr * cp * sy,
|
||||
cr * cp * sy - sr * sp * cy,
|
||||
cr * cp * cy + sr * sp * sy,
|
||||
)
|
||||
@@ -0,0 +1,316 @@
|
||||
"""仿真"抓取锁"（grasp lock）：让夹爪夹住的物体在仿真里真的不会滑掉。
|
||||
|
||||
=============================================================================
|
||||
它在链路上的位置
|
||||
=============================================================================
|
||||
真机：夹爪 46N 握力 + 指垫摩擦 → 工件被"抱住"，机械臂怎么动都不掉。
|
||||
仿真：夹持力 = 位置误差 × 接触刚度（见 gauge/model.sdf 的 kp 说明）。
|
||||
规站着不动时够用；一旦 S01 绕工具轴快转 150°、或 pre_lift 走 60~100mm
|
||||
的连续轨迹，规就在指间打滑（实测工具长度 0.243m → 0.285m、歪 62°），
|
||||
S07 随后拿到的几何全错 —— 现象是"判不到接触"或"规掉了(E1)"。
|
||||
|
||||
本模块做的唯一一件事：**在夹爪确认夹住时，用 DetachableJoint 插件把物体
|
||||
与手指用一个固定关节锁起来；夹爪张开时解锁。**
|
||||
|
||||
这不是"作弊绕过物理"：刚性接触求解器给不出静摩擦迟滞，也不模拟指垫橡胶
|
||||
的贴合，而真实夹持恰恰靠这两件事。锁补的就是这一块。物体与指垫之间的
|
||||
**接触对仍然存在**，所以触觉、腕部 FT 的读数照旧是真实的接触力。
|
||||
|
||||
=============================================================================
|
||||
为什么用 `ign topic` 命令行而不是 ros_gz_bridge
|
||||
=============================================================================
|
||||
触发消息是 `ignition.msgs.Empty`。bridging 一个"空消息"要走
|
||||
ros_gz_bridge 的 Empty 映射，而这条链路上**没有任何东西需要它出现在 ROS
|
||||
侧** —— 唯一的目的就是把 gz 的那个插件叫一下。多接一个 bridge 就多一个
|
||||
"配错了不报错"的点（bridge 类型名对不上时它只是安静地什么也不桥）。
|
||||
|
||||
代价是每次触发起一个短命进程（几十毫秒）。触发频率是"每次夹/放一次"，
|
||||
不是每个控制周期 —— 这个代价可以忽略。
|
||||
|
||||
=============================================================================
|
||||
静默失效的四个点（改配置时全都要对上）
|
||||
=============================================================================
|
||||
① <child_model> = 机器人 spawn 的 -name（launch 里 rizon_sim）
|
||||
② <child_link> = Gazebo 里**真实存在**的连杆名（URDF 里与固定关节相连的
|
||||
连杆会被 sdformat 合并，手指只剩 sim_left_finger_mount）
|
||||
③ attach/detach 话题名与 gauge/model.sdf 里的插件一致
|
||||
④ 插件确实写进了 gauge 的 SDF
|
||||
|
||||
①②③④ 全都不会自己报错：锁不触发时，现象只是"规莫名掉件"。
|
||||
所以 :func:`check_grasp_lock_sdf` 把它变成启动时的一条告警/报错。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import subprocess
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import yaml
|
||||
|
||||
#: 相对仓库根的默认配置路径。
|
||||
DEFAULT_GRASP_LOCK_REL = "devices/sim/gazebo/config/grasp_lock.yaml"
|
||||
|
||||
#: 被 lock 的物体模型 SDF 的默认位置（用来做静态核对）。
|
||||
DEFAULT_OBJECT_SDF_TMPL = "devices/sim/gazebo/models/{object}/model.sdf"
|
||||
|
||||
|
||||
class GraspLockError(RuntimeError):
|
||||
"""抓取锁配置缺失或写错。消息里必须给出修法（现场就靠这句话）。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GraspLockSpec:
|
||||
"""抓取锁的一份完整描述。"""
|
||||
|
||||
enabled: bool
|
||||
object: str
|
||||
object_link: str
|
||||
robot_model: str
|
||||
robot_link: str
|
||||
attach_topic: str
|
||||
detach_topic: str
|
||||
state_topic: str
|
||||
publish_timeout_ms: int
|
||||
|
||||
def describe(self) -> str:
|
||||
"""给启动日志用的一句话。"""
|
||||
if not self.enabled:
|
||||
return "抓取锁：关闭（仿真靠摩擦夹持，机械臂快速运动时可能滑件）"
|
||||
return (
|
||||
f"抓取锁：开 → {self.object}.{self.object_link} 锁到 "
|
||||
f"{self.robot_model}.{self.robot_link}"
|
||||
f"（attach={self.attach_topic} detach={self.detach_topic}）"
|
||||
)
|
||||
|
||||
|
||||
def load_grasp_lock(
|
||||
path: Optional[Path] = None, root: Optional[Path] = None
|
||||
) -> GraspLockSpec:
|
||||
"""读抓取锁配置。找不到就返回一份"关闭"的默认值（仿真仍能跑）。
|
||||
|
||||
**为什么文件缺失不报错**：抓取锁是仿真里的**补救措施**，不是必需品 ——
|
||||
关掉它只是回到"靠摩擦夹持"，多数轻动作仍然能跑。让仿真因为一个可选补救
|
||||
措施起不来，是拿主流程给可选项陪葬。
|
||||
但**文件存在而字段写错**时必须报错：那说明有人动过它、而且写错了。
|
||||
"""
|
||||
if path is None:
|
||||
base = root if root is not None else Path.cwd()
|
||||
path = Path(base) / DEFAULT_GRASP_LOCK_REL
|
||||
path = Path(path)
|
||||
|
||||
if not path.is_file():
|
||||
return GraspLockSpec(
|
||||
enabled=False,
|
||||
object="gauge",
|
||||
object_link="body",
|
||||
robot_model="",
|
||||
robot_link="",
|
||||
attach_topic="/sim/grasp_lock/attach",
|
||||
detach_topic="/sim/grasp_lock/detach",
|
||||
state_topic="/sim/grasp_lock/state",
|
||||
publish_timeout_ms=3000,
|
||||
)
|
||||
|
||||
try:
|
||||
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except yaml.YAMLError as exc:
|
||||
raise GraspLockError(
|
||||
f"抓取锁配置 YAML 解析失败: {path}\n 原因: {exc}"
|
||||
) from exc
|
||||
|
||||
if not isinstance(doc, dict):
|
||||
raise GraspLockError(f"{path} 的顶层应该是映射（key: value 形式）。")
|
||||
|
||||
enabled = bool(doc.get("enabled", True))
|
||||
|
||||
def _text(key: str, default: str = "") -> str:
|
||||
return str(doc.get(key, default) or "").strip()
|
||||
|
||||
spec = GraspLockSpec(
|
||||
enabled=enabled,
|
||||
object=_text("object", "gauge") or "gauge",
|
||||
object_link=_text("object_link", "body") or "body",
|
||||
robot_model=_text("robot_model"),
|
||||
robot_link=_text("robot_link"),
|
||||
attach_topic=_text("attach_topic", "/sim/grasp_lock/attach"),
|
||||
detach_topic=_text("detach_topic", "/sim/grasp_lock/detach"),
|
||||
state_topic=_text("state_topic", "/sim/grasp_lock/state"),
|
||||
publish_timeout_ms=int(doc.get("publish_timeout_ms", 3000)),
|
||||
)
|
||||
|
||||
if not enabled:
|
||||
return spec
|
||||
|
||||
missing = [
|
||||
k
|
||||
for k in ("robot_model", "robot_link", "attach_topic", "detach_topic")
|
||||
if not getattr(spec, k)
|
||||
]
|
||||
if missing:
|
||||
raise GraspLockError(
|
||||
f"{path} 开着（enabled: true）但缺字段: {', '.join(missing)}。\n"
|
||||
f" robot_model 必须等于 launch 里 spawn 机器人的 -name（当前 rizon_sim）；\n"
|
||||
f" robot_link 必须是 Gazebo 里的**真名**（与固定关节相连的连杆会被\n"
|
||||
f" sdformat 合并，手指只剩 sim_left_finger_mount）。真名查法：\n"
|
||||
f" ros2 topic echo /sim/world_pose_info ← 看全部 child_frame_id\n"
|
||||
f" 这两个名字写错的后果是**静默失效**：锁不触发、规照样滑掉，\n"
|
||||
f" 而日志里只看得到「规掉了」。"
|
||||
)
|
||||
if spec.attach_topic == spec.detach_topic:
|
||||
raise GraspLockError(
|
||||
f"{path} 的 attach_topic 与 detach_topic 相同（{spec.attach_topic}）——\n"
|
||||
f" 那样夹住和放开会走同一条消息，锁的状态完全随机。两个必须不同。"
|
||||
)
|
||||
if spec.publish_timeout_ms <= 0:
|
||||
raise GraspLockError(
|
||||
f"{path} 的 publish_timeout_ms 必须为正，收到 {spec.publish_timeout_ms}"
|
||||
)
|
||||
return spec
|
||||
|
||||
|
||||
def bridge_target_links(spec: GraspLockSpec) -> List[str]:
|
||||
"""把"锁的两端"整理成一条便于核对/记录的清单（给日志与自检用）。"""
|
||||
return [f"{spec.object}.{spec.object_link}", f"{spec.robot_model}.{spec.robot_link}"]
|
||||
|
||||
|
||||
def check_grasp_lock_sdf(
|
||||
sdf_text: str, spec: GraspLockSpec, sdf_label: str = ""
|
||||
) -> List[str]:
|
||||
"""静态核对：物体 SDF 里的 DetachableJoint 插件是否与本配置一致。
|
||||
|
||||
为什么值得一查（与腕部 FT 的 check 同一个理由）：
|
||||
插件参数写错时 Gazebo **什么都不说** —— 插件照样加载（只要 child_model
|
||||
找得到），assess 话题照样发，只是关节连不上。现象只有一个：
|
||||
"规莫名其妙掉了"，而那要等到 S07/S08 才显形、且看不到真因。
|
||||
这里在**启动时**把它变成一句话。
|
||||
|
||||
返回告警列表（空 = 没问题）；enabled=false 时永远返回空。
|
||||
"""
|
||||
if not spec.enabled:
|
||||
return []
|
||||
where = f"（{sdf_label}）" if sdf_label else ""
|
||||
warnings: List[str] = []
|
||||
if not sdf_text:
|
||||
return [f"读不到物体 SDF{where}，无法核对抓取锁插件"]
|
||||
|
||||
if "DetachableJoint" not in sdf_text:
|
||||
warnings.append(
|
||||
f"物体 SDF{where} 里没有 DetachableJoint 插件 —— 抓取锁不会生效。\n"
|
||||
f" 配置说 enabled: true，但模型里没有插件时**不会报错**，\n"
|
||||
f" 只是夹住的物体依旧会滑（现象＝「规莫名掉了」，看不到真因）。\n"
|
||||
f" 修法：把插件块补进 {spec.object}/model.sdf，参数照 grasp_lock.yaml。"
|
||||
)
|
||||
return warnings
|
||||
|
||||
if spec.robot_model and f"<child_model>{spec.robot_model}</child_model>" not in sdf_text:
|
||||
warnings.append(
|
||||
f"插件里的 <child_model> 不是 {spec.robot_model} —— "
|
||||
f"它必须等于 launch 里 spawn 机器人的 -name。\n"
|
||||
f" 对不上时 DetachableJoint 在 Configure 阶段直接失败，"
|
||||
f"插件被卸载、{spec.state_topic} 也不会出现（这是唯一的线索）。"
|
||||
)
|
||||
|
||||
if spec.robot_link and f"<child_link>{spec.robot_link}</child_link>" not in sdf_text:
|
||||
warnings.append(
|
||||
f"插件里的 <child_link> 不是 {spec.robot_link}。\n"
|
||||
f" 注意这里要填 Gazebo 的**真名**：URDF 里与固定关节相连的连杆会被\n"
|
||||
f" sdformat 合并（lump），例如手指只剩 sim_left_finger_mount。\n"
|
||||
f" 名字写错时插件照样加载、attach 照样「成功」，但关节连到不存在的\n"
|
||||
f" 连杆上 → 锁不生效。真名查法：ros2 topic echo /sim/world_pose_info"
|
||||
)
|
||||
|
||||
for topic, key in ((spec.attach_topic, "attach_topic"), (spec.detach_topic, "detach_topic")):
|
||||
if topic and f"<{key}>{topic}</{key}>" not in sdf_text:
|
||||
warnings.append(
|
||||
f"插件里的 <{key}> 不是 {topic}。\n"
|
||||
f" 两侧话题名不一致时，锁的触发消息发到一个没人听的地址上 ——\n"
|
||||
f" 不报错，只是永远不锁。"
|
||||
)
|
||||
|
||||
return warnings
|
||||
|
||||
|
||||
def _ign_topic_cmd(exe: str, topic: str, timeout_ms: int) -> List[str]:
|
||||
"""拼一条 `ign topic` 发布命令（空消息）。
|
||||
|
||||
⚠️ ``-d``（duration 秒）不是可选的装饰 —— 少了它**大概率丢消息**：
|
||||
CLI 建立 publisher 与网络里发现订阅者需要一点时间，一条 fire-and-forget
|
||||
的消息在发现完成前就发完了。曾经写成 ``-w <秒>``（以为它等订阅者），
|
||||
而这个版本的 ``ign topic`` **没有 -w** —— 参数被当成多余的位置参数：
|
||||
"The following arguments were not expected: 3"
|
||||
于是"上锁/解锁"永远失败，而现象和"锁没生效"完全一样（规照样掉）。
|
||||
"""
|
||||
return [
|
||||
exe,
|
||||
"topic",
|
||||
"-t", topic,
|
||||
"-m", "ignition.msgs.Empty",
|
||||
"-p", "",
|
||||
"-d", f"{max(0.5, timeout_ms / 1000.0):.2f}",
|
||||
]
|
||||
|
||||
|
||||
def _find_ign_exe(explicit: Optional[str] = None) -> Optional[str]:
|
||||
"""找到 ign / gz 命令行。找不到返回 None（调用方决定是告警还是忽略）。"""
|
||||
import shutil
|
||||
|
||||
if explicit:
|
||||
return explicit
|
||||
for name in ("ign", "gz"):
|
||||
found = shutil.which(name)
|
||||
if found:
|
||||
return found
|
||||
return None
|
||||
|
||||
|
||||
def publish_lock_event(
|
||||
spec: GraspLockSpec,
|
||||
attach: bool,
|
||||
exe: Optional[str] = None,
|
||||
logger=None,
|
||||
) -> bool:
|
||||
"""发一条"上锁/解锁"消息。返回是否成功（失败只记日志，不抛）。
|
||||
|
||||
**为什么不抛异常**：这不是主流程的一步 —— 它是仿真里的补救措施。
|
||||
发不出去时正确行为是"继续跑，但明确告警"，而不是把整条装配流程打断：
|
||||
现场宁可看到"锁没生效、规可能滑"的提示，也不要看到"因为锁发不出去，
|
||||
装配根本没做"。
|
||||
"""
|
||||
if not spec.enabled:
|
||||
return False
|
||||
topic = spec.attach_topic if attach else spec.detach_topic
|
||||
what = "上锁" if attach else "解锁"
|
||||
exe_path = _find_ign_exe(exe)
|
||||
if exe_path is None:
|
||||
if logger is not None:
|
||||
logger.warn(
|
||||
f"抓取锁{what}失败：找不到 ign/gz 命令行（PATH 里没有）。\n"
|
||||
f" 后果：夹住的物体在机械臂连续运动时可能滑掉（现象＝「规掉了」）。"
|
||||
)
|
||||
return False
|
||||
|
||||
cmd = _ign_topic_cmd(exe_path, topic, spec.publish_timeout_ms)
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
cmd,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.STDOUT,
|
||||
timeout=max(2.0, spec.publish_timeout_ms / 1000.0 + 2.0),
|
||||
)
|
||||
except (OSError, subprocess.SubprocessError) as exc:
|
||||
if logger is not None:
|
||||
logger.warn(f"抓取锁{what}失败（{topic}）：{exc}")
|
||||
return False
|
||||
|
||||
if proc.returncode != 0:
|
||||
out = (proc.stdout or b"").decode("utf-8", "replace").strip()
|
||||
if logger is not None:
|
||||
logger.warn(
|
||||
f"抓取锁{what}失败（{topic}，退出码 {proc.returncode}）：{out[:200]}"
|
||||
)
|
||||
return False
|
||||
if logger is not None:
|
||||
logger.info(f"抓取锁{what}：{topic}")
|
||||
return True
|
||||
@@ -0,0 +1,618 @@
|
||||
"""仿真夹爪的耦合关系、行程换算与命令向量拼装（纯逻辑，不起 ROS）。
|
||||
|
||||
=============================================================================
|
||||
为什么单独一个模块
|
||||
=============================================================================
|
||||
把"夹爪服务（mm）"变成"7 个关节的目标角（弧度/米）"，真正有内容的是四件事：
|
||||
|
||||
1. 耦合关系 从动关节 = multiplier × 主关节 + offset
|
||||
（上游 GN01 描述里主关节是 ``finger_width_joint``，
|
||||
6 个从动关节按各自的 multiplier 跟随）
|
||||
2. 行程换算 应用层说的是**开口毫米**，Gazebo 要的是**关节值**（米）
|
||||
3. 命令顺序 JointGroupPositionController 收到的数组顺序必须与
|
||||
controllers.yaml 里 ``joints:`` 完全一致
|
||||
4. 夹住判据 真机有状态字，仿真只能靠"指令到了下限、实际值没跟上"推断
|
||||
|
||||
四件都是纯数据变换，不需要 ROS 跑起来就能验。所以实现放这里、用普通函数表达，
|
||||
节点（sim_gripper_adapter.py）只做订阅/发布这层壳。这样"multiplier 用错"
|
||||
这种错误才能被单测钉住，而不是靠"看起来夹住了"。
|
||||
|
||||
=============================================================================
|
||||
为什么 Gazebo 里必须由适配器驱动 7 个关节
|
||||
=============================================================================
|
||||
sdformat 把 URDF 的 ``<mimic>`` **整个丢掉**，gz-sim 也不实现 mimic。
|
||||
所以仿真里 6 个从动关节不会跟随主关节，必须由适配器算出角度后**一起下发**。
|
||||
``urdf_normalize`` 在归一化时把 ``<mimic>`` 删掉（避免 robot_state_publisher
|
||||
和 Gazebo 各说各话），删掉的信息记在 ``NormalizeReport.stripped_mimics`` 里，
|
||||
launch 再把那张表作为参数交给本适配器 —— 这样"耦合关系"只有一个来源。
|
||||
|
||||
=============================================================================
|
||||
毫米 ↔ 关节值的口径（最容易搞错的一处）
|
||||
=============================================================================
|
||||
``finger_width_joint`` 是上游描述里的**虚拟宽度关节**（注释说它"本应是
|
||||
prismatic，但为了配合 mimic 只能是 revolute"），单位是**米**，限位 **0~0.100**，
|
||||
语义就是开口宽度：0 = 两指合拢，0.100 = 开口 100 mm。
|
||||
|
||||
本适配器采用**绝对（物理）映射**：命令"开口 8 mm"，仿真里两指就真的相距 8 mm。
|
||||
（而不是归一化映射 —— 那种把"100% 行程"当成全开，会让中间开口全部失真，
|
||||
仿真就失去"拿真几何试算法"的意义。）
|
||||
|
||||
⚠️ **但这里有个坑：不能简单地写成 `关节值 = 开口_mm / 1000`。**
|
||||
上游的耦合是**线性近似**，而机构本身是非线性的（手指是"转 + 再转"，指垫
|
||||
既平移又旋转）。按线性口径命令 20 mm，Gazebo 实测两指相距 **21.75 mm** ——
|
||||
抓 20 mm 的工件会空出 1.75 mm，没有接触力，触觉那一步在仿真里直接废掉。
|
||||
|
||||
所以开口↔关节值走**URDF 正运动学反解**（见 sim_gripper_kin.py）：
|
||||
|
||||
aperture_to_source(20.0) → 令两指间距 − 合拢间距 == 20.0 的关节值
|
||||
|
||||
这个正运动学与 Gazebo 的实测吻合到 0.01 mm（0~100 mm 全程），两边完全独立
|
||||
（一个是矩阵乘法，一个是物理引擎），所以"开口 20 mm"在仿真里是物理上真的 20 mm。
|
||||
顺带一个推论：GN01 的**实测**行程是 99.93 mm，不是名义的 100.0 ——
|
||||
启动日志里报的是实测值。
|
||||
|
||||
万一某个夹爪的指尖 link 命名不符合约定，正运动学建不出来，会**退回线性近似**
|
||||
并在启动日志里明确写出口径变了（中段偏差可达 4 mm/100mm 行程）。
|
||||
别在那种情况下拿仿真去标触觉阈值。
|
||||
|
||||
⚠️ 还要注意**仿真夹爪和真机夹爪不是同一个夹爪**：仿真是官方 GN01（行程
|
||||
100 mm），现场是 DH PGEA-100-20-W（``site.yaml`` 的 ``gripper.stroke_mm``
|
||||
= 20 mm）。同一个"8 mm 开口"在两者上都物理正确，但"全开"对应的开口
|
||||
完全不同。适配器启动时会把两者的行程打出来，避免有人默认它们一样。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Mapping, Optional, Sequence, Tuple
|
||||
|
||||
from .sim_gripper_kin import ApertureCurve, KinematicsError, UrdfTree, find_tip_pair
|
||||
|
||||
import yaml
|
||||
|
||||
# 默认控制器配置位置（相对仓库根）。与真机那份同名不同目录，别搞混。
|
||||
DEFAULT_CONTROLLERS_REL = "devices/sim/gazebo/config/controllers.yaml"
|
||||
|
||||
#: 适配器负责的控制器名。改这里要同步改 launch 与 controllers.yaml。
|
||||
DEFAULT_CONTROLLER = "gripper_controller"
|
||||
|
||||
#: 关节值（米）与开口（毫米）的换算系数。
|
||||
MM_PER_M = 1000.0
|
||||
|
||||
#: 参数里 mimic 表的序列化分隔符。关节名里不会有 ':'，所以安全。
|
||||
_MIMIC_SEP = ":"
|
||||
|
||||
|
||||
class GripperConfigError(RuntimeError):
|
||||
"""夹爪配置/描述有问题时抛出，消息面向使用者。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MimicRelation:
|
||||
"""一条 ``<mimic>`` 耦合关系：从动关节 = multiplier × 主关节 + offset。"""
|
||||
|
||||
joint: str
|
||||
source: str
|
||||
multiplier: float
|
||||
offset: float
|
||||
|
||||
def apply(self, source_value: float) -> float:
|
||||
"""按这条关系算出从动关节的值。"""
|
||||
return self.multiplier * source_value + self.offset
|
||||
|
||||
def to_param(self) -> str:
|
||||
"""序列化成 ROS 参数用的字符串（launch 与本模块必须用同一格式）。"""
|
||||
return format_mimic_param(self.joint, self.source, self.multiplier, self.offset)
|
||||
|
||||
|
||||
def format_mimic_param(joint: str, source: str, multiplier: object, offset: object) -> str:
|
||||
"""把一条耦合关系序列化成 ROS 参数用的字符串。
|
||||
|
||||
launch 从 ``NormalizeReport.stripped_mimics`` 拿到的是**四个字符串**，
|
||||
本模块解析的也是这个格式，所以格式化只留这一个实现 ——
|
||||
两边各写一份的话，改了格式就会一边发一边收不到（而且不报错）。
|
||||
"""
|
||||
return _MIMIC_SEP.join(
|
||||
(str(joint).strip(), str(source).strip(), repr(float(multiplier)), repr(float(offset)))
|
||||
)
|
||||
|
||||
|
||||
class GripperLayout:
|
||||
"""夹爪在仿真里的完整描述：关节顺序 + 耦合关系 + 主关节限位。
|
||||
|
||||
由 :func:`layout_from_parts` 构造（它做全部校验）；不要直接 new。
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
order: Sequence[str],
|
||||
relations: Mapping[str, MimicRelation],
|
||||
source: str,
|
||||
lower: float,
|
||||
upper: float,
|
||||
curve: Optional[ApertureCurve] = None,
|
||||
kin_note: str = "",
|
||||
) -> None:
|
||||
self.order: Tuple[str, ...] = tuple(order)
|
||||
self.relations: Dict[str, MimicRelation] = dict(relations)
|
||||
self.source = source
|
||||
self.lower = float(lower)
|
||||
self.upper = float(upper)
|
||||
self.feedback = self._pick_feedback()
|
||||
# 由 URDF 正运动学给出的"真实开口曲线"；None = 退回线性近似。
|
||||
self.curve: Optional[ApertureCurve] = curve
|
||||
#: 口径说明（给日志用）：是 FK 精确口径还是线性近似。
|
||||
self.kin_note: str = kin_note
|
||||
|
||||
# --- 反馈关节 ---------------------------------------------------------
|
||||
def _pick_feedback(self) -> str:
|
||||
"""挑一个**真的会动**的关节来当开口反馈源。
|
||||
|
||||
⚠️ 主关节 ``finger_width_joint`` 修不出来：上游描述里它挂在一个叫
|
||||
``unused_finger_width_link`` 的**孤立假 link** 上（注释原话：
|
||||
"The unused link cannot have 0 mass"），和手指没有任何机械连接。
|
||||
实测（2026-09-24，Gazebo Fortress + gz_ros2_control）：命令它到 0.100，
|
||||
它的状态**始终停在 0**，而 6 个从动关节准确到位。
|
||||
所以拿主关节当反馈源的话，``status`` 会永远报"开口 0 mm"，
|
||||
而夹爪明明开着 —— 这正是"看起来在工作"的静默失效。
|
||||
|
||||
于是改从**与主关节直接相连的那个从动关节**反推开口：
|
||||
state = m × source + o → source = (state - o) / m
|
||||
选 |m| 最大的那个（分辨率最好；m 为 0 的不能用）。
|
||||
|
||||
单关节夹爪（没有耦合关系）时退回到主关节自己。
|
||||
"""
|
||||
candidates = [
|
||||
(abs(rel.multiplier), joint)
|
||||
for joint, rel in self.relations.items()
|
||||
if rel.source == self.source and abs(rel.multiplier) > 1e-9
|
||||
]
|
||||
if not candidates:
|
||||
return self.source
|
||||
candidates.sort(reverse=True)
|
||||
return candidates[0][1]
|
||||
|
||||
def feedback_value(self, source_value: float) -> float:
|
||||
"""反馈关节在"主关节 = source_value"时应有的值（用于判"没跟上"）。"""
|
||||
if self.feedback == self.source:
|
||||
return float(source_value)
|
||||
return self.value_for(self.feedback, source_value)
|
||||
|
||||
def source_from_feedback(self, feedback_state: float) -> float:
|
||||
"""反馈关节实测值 → 等价的主关节值（米）。"""
|
||||
if self.feedback == self.source:
|
||||
return float(feedback_state)
|
||||
rel = self.relations[self.feedback]
|
||||
if abs(rel.multiplier) < 1e-9:
|
||||
raise GripperConfigError(
|
||||
f"反馈关节 {self.feedback!r} 的 multiplier 为 0，无法反推主关节值。"
|
||||
)
|
||||
return (float(feedback_state) - rel.offset) / rel.multiplier
|
||||
|
||||
def aperture_from_feedback(self, feedback_state: float) -> float:
|
||||
"""反馈关节的实测值 → 开口（毫米）。"""
|
||||
return self.source_to_aperture(self.source_from_feedback(feedback_state))
|
||||
|
||||
# --- 行程口径 ---------------------------------------------------------
|
||||
@property
|
||||
def stroke_mm(self) -> float:
|
||||
"""这个夹爪在仿真里**实际**能开的宽度（毫米）。
|
||||
|
||||
有 FK 曲线时用实测行程（GN01 ≈ 99.93 mm，不是名义的 100.0 ——
|
||||
上游把宽度关节限位定在 0~0.100 m，而线性耦合让指尖只走了 99.93，
|
||||
报名义值会让"全开时开口读数与几何对不上"变成一个查不出的矛盾）。
|
||||
没有 FK 曲线时只能按限位换算，并在 :attr:`kin_note` 里说明是近似。
|
||||
|
||||
真机那份行程在 ``site.yaml`` 的 ``gripper.stroke_mm``；两者不同是正常的
|
||||
（不同夹爪），但要显式打出来，见 :func:`stroke_note`。
|
||||
"""
|
||||
if self.curve is not None:
|
||||
return self.curve.stroke_mm
|
||||
return (self.upper - self.lower) * MM_PER_M
|
||||
|
||||
@property
|
||||
def closed_separation_mm(self) -> Optional[float]:
|
||||
"""合拢时两侧 tip frame 的固有间距（GN01 ≈ 27 mm）；无 FK 时为 None。
|
||||
|
||||
验收脚本用它把"指尖间距"折成"开口"（指垫合拢时间距不为 0，
|
||||
因为 tip frame 不在指垫表面）。见 scripts/check_sim_gripper.py。
|
||||
"""
|
||||
return None if self.curve is None else self.curve.closed_separation_mm
|
||||
|
||||
def aperture_to_source(self, aperture_mm: float) -> float:
|
||||
"""开口（毫米）→ 主关节值（米），并按限位截断。
|
||||
|
||||
**绝对物理映射**：夹爪真的开这么宽。越界不报错而是截断（撞到限位是
|
||||
正常操作），但调用方可以从 :meth:`is_clamped` 知道发生了截断。
|
||||
|
||||
⚠️ 有 FK 曲线时走**反解**，不是线性换算。因为上游的耦合是线性近似、
|
||||
而机构是非线性的：线性口径下"命令 20 mm"实际是 21.75 mm 的指垫间距，
|
||||
抓 20 mm 的工件会空出 1.75 mm（没有接触力），
|
||||
触觉那一步在仿真里就废了。细节见 sim_gripper_kin.py 文件头。
|
||||
"""
|
||||
if self.curve is not None:
|
||||
return self.curve.joint_value(aperture_mm)
|
||||
raw = self.lower + float(aperture_mm) / MM_PER_M
|
||||
return min(max(raw, self.lower), self.upper)
|
||||
|
||||
def source_to_aperture(self, source_value: float) -> float:
|
||||
"""主关节值（米）→ 开口（毫米）。与 :meth:`aperture_to_source` 互逆。"""
|
||||
if self.curve is not None:
|
||||
return self.curve.aperture_mm(source_value)
|
||||
return (float(source_value) - self.lower) * MM_PER_M
|
||||
|
||||
def is_clamped(self, aperture_mm: float) -> bool:
|
||||
"""这个开口请求是否会被限位截断（用来给调用方一句实话）。"""
|
||||
if self.curve is not None:
|
||||
return self.curve.is_clamped(aperture_mm)
|
||||
raw = self.lower + float(aperture_mm) / MM_PER_M
|
||||
return not (self.lower <= raw <= self.upper)
|
||||
|
||||
# --- 命令向量 ---------------------------------------------------------
|
||||
def command_vector(self, aperture_mm: float) -> List[float]:
|
||||
"""按 ``order`` 拼出一整组关节目标（主关节 + 从动关节）。
|
||||
|
||||
顺序必须与 controllers.yaml 的 ``joints:`` 一致 —— 顺序错了，
|
||||
JointGroupPositionController 不会报错，只会把角度发给**别的关节**，
|
||||
表现为"夹爪动得很奇怪"。
|
||||
"""
|
||||
return self.command_vector_from_source(self.aperture_to_source(aperture_mm))
|
||||
|
||||
def command_vector_from_source(self, source_value: float) -> List[float]:
|
||||
"""同上，但直接给主关节值（米），不经过"毫米"这一层来回换算。
|
||||
|
||||
``stop``（保持当前位置）这类命令手里只有关节状态值，走毫米会在
|
||||
两次换算里引入舍入误差，所以单独留这个入口。
|
||||
"""
|
||||
return [self.value_for(name, source_value) for name in self.order]
|
||||
|
||||
def value_for(self, joint: str, source_value: float) -> float:
|
||||
"""某个关节在"主关节 = source_value"时应有的值。
|
||||
|
||||
主关节自己就是 ``source_value``；从动关节沿**耦合链**逐级算出来。
|
||||
|
||||
⚠️ GN01 的耦合不是"6 个手指关节全部直挂主关节"，而是一条链：
|
||||
|
||||
finger_width_joint ─9.404,-0.155→ left_outer_knuckle_joint
|
||||
├─ 1, 0 → left_inner_knuckle
|
||||
├─ -1,0 → left_inner_finger
|
||||
├─ 1, 0 → right_outer_knuckle
|
||||
├─ 1, 0 → right_inner_knuckle
|
||||
└─ -1,0 → right_inner_finger
|
||||
|
||||
所以必须**逐级**套用 multiplier/offset（先套靠近根的、再套靠近叶的），
|
||||
不能只套一层。只套一层的话，指尖角度会错得不多但一直不对 ——
|
||||
表现为"夹爪动作看着像那么回事，就是夹不准"。
|
||||
"""
|
||||
if joint == self.source:
|
||||
return float(source_value)
|
||||
|
||||
# 从 joint 往上爬到根，把沿途的关系按"从根到叶"的顺序套用。
|
||||
chain: List[MimicRelation] = []
|
||||
seen = set()
|
||||
cur = joint
|
||||
while True:
|
||||
rel = self.relations.get(cur)
|
||||
if rel is None:
|
||||
break
|
||||
if cur in seen:
|
||||
raise GripperConfigError(
|
||||
f"耦合链成环：{cur!r} 又回到了自己（{' → '.join(sorted(seen))}）"
|
||||
)
|
||||
seen.add(cur)
|
||||
chain.append(rel)
|
||||
cur = rel.source
|
||||
if cur != self.source:
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint!r} 的耦合链终止在 {cur!r}，而不是主关节 "
|
||||
f"{self.source!r} —— 描述里的耦合表不完整。"
|
||||
)
|
||||
value = float(source_value)
|
||||
for rel in reversed(chain):
|
||||
value = rel.apply(value)
|
||||
return value
|
||||
|
||||
def source_index(self) -> int:
|
||||
"""主关节在命令数组里的下标。"""
|
||||
return self.order.index(self.source)
|
||||
|
||||
def _source_limits_note(self) -> str:
|
||||
return f"限位 [{self.lower:.4f}, {self.upper:.4f}] m"
|
||||
|
||||
def describe(self) -> str:
|
||||
"""启动时打给操作者看的一行摘要（含必须知道的口径）。"""
|
||||
note = f"；{self.kin_note}" if self.kin_note else ""
|
||||
return (
|
||||
f"主关节={self.source} {self._source_limits_note()} "
|
||||
f"→ 仿真行程 {self.stroke_mm:.2f} mm；"
|
||||
f"共 {len(self.order)} 个关节，{len(self.relations)} 条耦合{note}"
|
||||
)
|
||||
|
||||
def stroke_note(self, real_stroke_mm: Optional[float]) -> str:
|
||||
"""仿真行程与真机行程的对比说明（两者不同是正常的，但必须说出来）。"""
|
||||
if real_stroke_mm is None or real_stroke_mm <= 0:
|
||||
return (
|
||||
f"仿真夹爪行程 {self.stroke_mm:.1f} mm（site.yaml 里没读到真机行程，"
|
||||
f"无法对比）"
|
||||
)
|
||||
same = abs(real_stroke_mm - self.stroke_mm) < 0.5
|
||||
head = f"仿真行程 {self.stroke_mm:.1f} mm vs 真机 {real_stroke_mm:.1f} mm"
|
||||
if same:
|
||||
return head + "：一致。"
|
||||
return (
|
||||
head + "：**不一致**（不同型号的夹爪，这是正常的）。\n"
|
||||
" 开口毫米采用绝对物理映射，所以同一个开口在两边几何上都对；\n"
|
||||
" 但\"全开\"对应的开口不同，别把仿真里的全开当成真机的全开。\n"
|
||||
" 力/触觉阈值也要按各自夹爪重新标定（见 R3.5）。"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 解析与校验
|
||||
# ---------------------------------------------------------------------------
|
||||
def parse_mimics(entries: Sequence[str]) -> Dict[str, MimicRelation]:
|
||||
"""解析参数里的 mimic 表（每条 ``joint:source:multiplier:offset``）。
|
||||
|
||||
格式错就报错，不做"跳过这一条"的宽容处理：静默丢一条耦合关系，
|
||||
结果是某个手指关节永远不动，而 Gazebo 不会抱怨。
|
||||
"""
|
||||
out: Dict[str, MimicRelation] = {}
|
||||
for raw in entries:
|
||||
parts = str(raw).split(_MIMIC_SEP)
|
||||
if len(parts) != 4:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数格式不对：{raw!r}\n"
|
||||
f" 应为 '从动关节:主关节:multiplier:offset' 四段。"
|
||||
)
|
||||
joint, source, mult_s, off_s = (p.strip() for p in parts)
|
||||
if not joint or not source:
|
||||
raise GripperConfigError(f"mimic 参数里关节名为空：{raw!r}")
|
||||
try:
|
||||
mult, off = float(mult_s), float(off_s)
|
||||
except ValueError as exc:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数里的 multiplier/offset 不是数字：{raw!r}"
|
||||
) from exc
|
||||
if joint == source:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数里从动关节与主关节同名（{joint}）—— 描述有问题。"
|
||||
)
|
||||
if joint in out:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数里关节 {joint!r} 出现了两次，无法确定用哪条。"
|
||||
)
|
||||
out[joint] = MimicRelation(joint, source, mult, off)
|
||||
return out
|
||||
|
||||
|
||||
def gripper_joint_order(
|
||||
controllers_file: Path, controller: str = DEFAULT_CONTROLLER
|
||||
) -> List[str]:
|
||||
"""从 controllers.yaml 里读 ``<controller>.ros__parameters.joints``。
|
||||
|
||||
为什么解析而不是让调用方再配一份顺序：顺序是
|
||||
``JointGroupPositionController`` 的硬契约，配两份必然漂移，
|
||||
而漂移的症状是"角度发给了别的关节"、不报错。
|
||||
"""
|
||||
path = Path(controllers_file)
|
||||
try:
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except OSError as exc:
|
||||
raise GripperConfigError(f"读不到控制器配置 {path}：{exc}") from exc
|
||||
except yaml.YAMLError as exc:
|
||||
raise GripperConfigError(f"控制器配置 {path} 不是合法 YAML：{exc}") from exc
|
||||
|
||||
section = (data.get(controller) or {}).get("ros__parameters")
|
||||
if not isinstance(section, dict):
|
||||
raise GripperConfigError(
|
||||
f"{path} 里没有 {controller}.ros__parameters 段。\n"
|
||||
f" 仿真夹爪要靠这个控制器的 joints 顺序下发命令，缺了就无法工作。"
|
||||
)
|
||||
joints = section.get("joints")
|
||||
if not isinstance(joints, list) or not joints:
|
||||
raise GripperConfigError(
|
||||
f"{path} 的 {controller}.ros__parameters 里 joints 为空。"
|
||||
)
|
||||
names = [str(j).strip() for j in joints]
|
||||
if len(set(names)) != len(names):
|
||||
raise GripperConfigError(
|
||||
f"{path} 的 {controller} joints 里有重复项：{names}"
|
||||
)
|
||||
return names
|
||||
|
||||
|
||||
def joint_limits(urdf_xml: str, joint_name: str) -> Tuple[float, float]:
|
||||
"""从 URDF 文本里取某个关节的 (lower, upper)。取不到就报错。
|
||||
|
||||
为什么不从参数再传一遍限位：限位是描述里的**唯一真相**（也就是 Gazebo
|
||||
里真实的行程）。再配一份等于给自己一个"改了描述没改配置"的机会，
|
||||
而症状是开口悄悄不按预期走。
|
||||
"""
|
||||
# 只认 <joint name="..."> 那一段，避免匹配到别的关节。
|
||||
pat = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*[\"']%s[\"'][^>]*>(.*?)</joint>"
|
||||
% re.escape(joint_name),
|
||||
re.DOTALL,
|
||||
)
|
||||
m = pat.search(urdf_xml or "")
|
||||
if not m:
|
||||
raise GripperConfigError(
|
||||
f"在 robot_description 里找不到关节 {joint_name!r}。\n"
|
||||
f" 常见原因：描述没带夹爪（load_gripper:=false），或关节名前缀不对"
|
||||
f"（仿真是 sim_ 前缀）。"
|
||||
)
|
||||
body = m.group(1)
|
||||
if re.search(r"<mimic\b", body):
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint_name!r} 自己是从动关节（带 <mimic>），不能当主关节。"
|
||||
)
|
||||
lm = re.search(r"<limit\b[^>]*>", body)
|
||||
if not lm:
|
||||
raise GripperConfigError(f"关节 {joint_name!r} 的 <limit> 标签缺失。")
|
||||
|
||||
def _attr(name: str) -> Optional[float]:
|
||||
am = re.search(r"\b%s\s*=\s*[\"']([^\"']+)[\"']" % name, lm.group(0))
|
||||
if not am:
|
||||
return None
|
||||
try:
|
||||
return float(am.group(1))
|
||||
except ValueError:
|
||||
return None
|
||||
|
||||
lower, upper = _attr("lower"), _attr("upper")
|
||||
if lower is None or upper is None:
|
||||
# URDF 允许省略 limit 的上下限（continuous 关节就没有）。夹爪的宽度
|
||||
# 关节必须有，否则我们无从知道"全开"是多少 —— 这时报错比猜好。
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint_name!r} 的 <limit> 缺 lower/upper，"
|
||||
f"无法确定开口行程。原始标签：{lm.group(0)}"
|
||||
)
|
||||
if upper <= lower:
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint_name!r} 的限位不合法：lower={lower} upper={upper}"
|
||||
)
|
||||
return lower, upper
|
||||
|
||||
|
||||
def layout_from_parts(
|
||||
order: Sequence[str],
|
||||
mimics: Mapping[str, MimicRelation],
|
||||
urdf_xml: str,
|
||||
) -> GripperLayout:
|
||||
"""把三份数据拼成 :class:`GripperLayout`，并做全部一致性校验。
|
||||
|
||||
校验点（每一条都对应一种"不报错但行为错"的故障）：
|
||||
|
||||
* ``mimics`` 里的从动关节必须在 ``order`` 里 —— 否则控制器根本没声明它，
|
||||
我们发出去的这一路会被忽略；
|
||||
* 从动关节引用的 ``source`` 也必须在 ``order`` 里 —— 链上有个关节没被
|
||||
控制器声明的话，那一级的角度就没人驱动；
|
||||
* 主关节（不带 mimic 的那个）必须**唯一** —— 多个候选说明推断逻辑不成立，
|
||||
这时静默挑第一个会挑错；
|
||||
* 每条耦合链都必须能走到主关节（不终止、成环都报错）—— 只校验开头几层
|
||||
的话，链尾错一处只表现为"某个指尖角度一直不对"。
|
||||
"""
|
||||
order = [str(j) for j in order]
|
||||
if not order:
|
||||
raise GripperConfigError("夹爪控制器没有声明任何关节。")
|
||||
|
||||
followers = dict(mimics)
|
||||
unknown = sorted(set(followers) - set(order))
|
||||
if unknown:
|
||||
raise GripperConfigError(
|
||||
f"mimic 表里的关节不在控制器 joints 列表里：{unknown}\n"
|
||||
f" 控制器声明的是：{order}\n"
|
||||
f" 两边必须一致，否则我们算出来的角度会被控制器忽略。"
|
||||
)
|
||||
missing_src = sorted({r.source for r in followers.values()} - set(order))
|
||||
if missing_src:
|
||||
raise GripperConfigError(
|
||||
f"mimic 表里有关节的**主关节**不在控制器 joints 列表里：{missing_src}\n"
|
||||
f" 控制器声明的是：{order}\n"
|
||||
f" 耦合链上少一环，下面所有关节的角度都会算错。"
|
||||
)
|
||||
|
||||
candidates = [j for j in order if j not in followers]
|
||||
if len(candidates) != 1:
|
||||
raise GripperConfigError(
|
||||
f"无法唯一确定主关节：控制器里有 {len(candidates)} 个关节不带 mimic "
|
||||
f"（{candidates}），期望恰好 1 个。\n"
|
||||
f" 通常是描述里夹爪的 mimic 表没传进来（load_gripper 关了？），"
|
||||
f"或者上游描述换了耦合方式。"
|
||||
)
|
||||
source = candidates[0]
|
||||
|
||||
lower, upper = joint_limits(urdf_xml, source)
|
||||
layout = GripperLayout(order, followers, source, lower, upper)
|
||||
|
||||
# 逐条把耦合链走一遍：成环 / 不终止在这里就报出来，别等到运行期。
|
||||
for joint in followers:
|
||||
layout.value_for(joint, 0.0)
|
||||
|
||||
# 开口曲线要在 layout 之后建：它调 layout.value_for 拿每个关节的角度，
|
||||
# 而 value_for 是耦合链的**唯一**权威实现 —— 曲线里再抄一份迟早会漂移。
|
||||
# （所以是"先建后回填"，不是构造参数。）
|
||||
layout.curve, layout.kin_note = _build_aperture_curve(layout, urdf_xml)
|
||||
return layout
|
||||
|
||||
|
||||
def _build_aperture_curve(
|
||||
layout: GripperLayout, urdf_xml: str
|
||||
) -> Tuple[Optional[ApertureCurve], str]:
|
||||
"""建"开口毫米 ↔ 主关节值"的精确曲线；建不出来就退回线性并**说明白**。
|
||||
|
||||
返回 ``(曲线或 None, 给日志的一句话)``。
|
||||
|
||||
为什么允许退回：换一个夹爪时，如果它的指尖 link 命名不符合约定，
|
||||
硬报错会让整个仿真起不来；而线性近似虽然中段有几毫米偏差，但
|
||||
"能不能动、方向对不对"仍然可验。所以退回，但**必须在日志里说清楚
|
||||
口径变成了近似** —— 否则有人会拿它当毫米精度的夹爪去标定触觉阈值。
|
||||
"""
|
||||
try:
|
||||
tree = UrdfTree(urdf_xml)
|
||||
tips = find_tip_pair(tree)
|
||||
if tips is None:
|
||||
return None, (
|
||||
"⚠ 没在 URDF 里认出左右指尖 link（按 *left*finger*tip* / "
|
||||
"*right*finger*tip* 约定找）→ 开口退回**线性近似**"
|
||||
"（中段偏差可达 4 mm/100mm 行程，别拿它标触觉阈值）"
|
||||
)
|
||||
curve = ApertureCurve(
|
||||
tree,
|
||||
tips,
|
||||
angles_of=lambda v: {j: layout.value_for(j, v) for j in layout.order},
|
||||
lower=layout.lower,
|
||||
upper=layout.upper,
|
||||
)
|
||||
return curve, (
|
||||
f"开口由 URDF 正运动学反解（{tips[0]} / {tips[1]}；"
|
||||
f"合拢时间距 {curve.closed_separation_mm:.2f} mm）"
|
||||
)
|
||||
except KinematicsError as exc:
|
||||
return None, (
|
||||
f"⚠ 正运动学不可用（{exc}）→ 开口退回**线性近似**"
|
||||
f"（中段偏差可达 4 mm/100mm 行程）"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 夹住判据
|
||||
# ---------------------------------------------------------------------------
|
||||
#: 判"夹住了"的默认容差（**毫米开口**）。GN01 全行程 100 mm，2 mm 的开口差
|
||||
#: 只可能是被挡住了，不可能是控制精度问题。
|
||||
DEFAULT_GRASP_TOLERANCE_MM = 2.0
|
||||
|
||||
#: 判"还在动"的默认容差（毫米开口）。比上面小：它只是用来区分 MOVING / IDLE。
|
||||
DEFAULT_MOVING_TOLERANCE_MM = 0.5
|
||||
|
||||
|
||||
def is_grasping(
|
||||
target_aperture_mm: float,
|
||||
actual_aperture_mm: float,
|
||||
tolerance_mm: float = DEFAULT_GRASP_TOLERANCE_MM,
|
||||
) -> bool:
|
||||
"""由"指令到了合拢位、实际开口却更大"推断夹住了东西。
|
||||
|
||||
真机（DH）有状态字可以问 "object caught"，仿真没有。但仿真里有一个等价
|
||||
且更本质的信号：位置控制器把目标压到合拢位，若两指之间夹着东西，
|
||||
关节就**到不了**那个位置，稳态开口会停在工件宽度附近。
|
||||
|
||||
两个条件同时成立才算夹住：
|
||||
|
||||
1. 目标在合拢侧（``target_aperture_mm`` 小于容差）—— 否则"有误差"只是没走到；
|
||||
2. 实际开口比目标大出容差 —— 被挡住了。
|
||||
|
||||
⚠️ 口径统一用**毫米开口**，不是主关节值（米）。因为开口↔关节值在
|
||||
FK 口径下是非线性的（见 sim_gripper_kin.py），拿米当口径的话"2 mm 容差"
|
||||
在不同开度下含义不同，标定出来的阈值换台夹爪就废了。
|
||||
|
||||
这是**推断**，不是测量：工件被夹在两指之间、但关节仍走到位的特殊情况
|
||||
（例如工件很薄且被压进夹爪根部）会漏判。所以 P6 的最终判据仍然用触觉
|
||||
力（见 R3.1），这里只提供 ``GripperStatus.is_gripped`` 这个粗信号。
|
||||
"""
|
||||
near_closed = float(target_aperture_mm) <= float(tolerance_mm)
|
||||
blocked = (
|
||||
float(actual_aperture_mm) - float(target_aperture_mm)
|
||||
) > float(tolerance_mm)
|
||||
return bool(near_closed and blocked)
|
||||
+515
@@ -0,0 +1,515 @@
|
||||
"""仿真夹爪适配器：``GripperExecute`` 服务 → ``JointGroupPositionController``。
|
||||
|
||||
=============================================================================
|
||||
它在链路上的位置
|
||||
=============================================================================
|
||||
应用层（P6 状态机 / 现场脚本）
|
||||
│ 服务 <ns>/open | <ns>/close | <ns>/move | <ns>/rotate
|
||||
│ （arm_msgs/srv/GripperExecute，与真机**同名同类型**）
|
||||
▼
|
||||
本节点
|
||||
│ ① 开口毫米 → 主关节值（绝对物理映射，见 sim_gripper.py 文件头）
|
||||
│ ② 按 mimic 耦合算出 6 个从动关节的目标角
|
||||
│ ③ 按 controllers.yaml 的 joints 顺序拼成数组
|
||||
▼
|
||||
话题 <controller>/commands (std_msgs/Float64MultiArray)
|
||||
▼
|
||||
JointGroupPositionController（gz_ros2_control，在 Gazebo 进程里）
|
||||
▼
|
||||
Gazebo 里的 7 个 sim_* 夹爪关节
|
||||
|
||||
同时订阅 /joint_states，把**反馈关节**的实际值换算回开口毫米，
|
||||
发布到 <ns>/status（arm_msgs/GripperStatus）。
|
||||
注意不是主关节 —— 主关节在 Gazebo 里压根不动，见下面第 4 个坑。
|
||||
|
||||
=============================================================================
|
||||
为什么必须由适配器驱动全部 7 个关节
|
||||
=============================================================================
|
||||
sdformat 把 URDF 的 ``<mimic>`` 整个丢掉，gz-sim 也不实现 mimic ——
|
||||
6 个从动关节在仿真里不会跟随主关节。所以适配器必须自己算。
|
||||
这件事同时解决了一个 TF 隐患：``robot_state_publisher`` 是按 /joint_states
|
||||
里的**真实值**发布 TF 的，只要 7 个关节都被驱动、且值符合耦合公式，
|
||||
RViz/TF 与 Gazebo 就一致（详见 urdf_normalize.py 里那段说明）。
|
||||
|
||||
=============================================================================
|
||||
⚠️ 仿真夹爪与真机夹爪不是同一个夹爪
|
||||
=============================================================================
|
||||
仿真是官方 Flexiv-GN01（行程 100 mm），现场是 DH PGEA-100-20-W
|
||||
（``site.yaml`` 的 ``gripper.stroke_mm`` = 20 mm）。开口用绝对物理映射，
|
||||
所以同一句"开 8 mm"两边都对；但"全开"对应的开口不同。启动时会把两者
|
||||
都打出来，差异大时会明确提示（见 :meth:`GripperLayout.stroke_note`）。
|
||||
|
||||
=============================================================================
|
||||
三个必须知道的坑
|
||||
=============================================================================
|
||||
1) **命令数组的顺序**取自 controllers.yaml，不是本节点里的常量。
|
||||
顺序错不会报错，只会把角度发给别的关节 —— 表现成"夹爪动得很奇怪"。
|
||||
所以本节点启动时先把顺序读出来并校验（从动关节必须在列表里）。
|
||||
|
||||
2) **``rotate`` 在平行夹爪上没有对应自由度**。真机那边 RGI 才有。这里明确
|
||||
返回 ``success=false`` 并说明原因，而不是假装成功 —— 假装成功会让上层
|
||||
的"拧入"流程在仿真里走到一个永远不动的分支上。
|
||||
|
||||
3) **``status`` 的 ``is_gripped`` 是推断值**（指令到合拢位但实际值没跟上），
|
||||
不是测量值。真机有 DH 状态字，仿真没有。最终夹持判据仍应看触觉力，
|
||||
见 sim_gripper.is_grasping 的说明。
|
||||
|
||||
4) **主关节 ``finger_width_joint`` 在 Gazebo 里不会动**。上游描述把它挂在
|
||||
孤立的 ``unused_finger_width_link`` 上（那个 link 的注释写着 "cannot have
|
||||
0 mass"，就是为了让 URDF 合法），它和手指没有任何机械连接。
|
||||
实测：命令到 0.100，它的 /joint_states 值恒为 ~0，而 6 个从动关节准确到位。
|
||||
所以：
|
||||
- 命令数组里**仍然要带上它**（controllers.yaml 的 joints 含它，少一个会错位）；
|
||||
- 但**反馈绝不能取它**，否则 status 永远报"开口 0 mm"而夹爪明明开着。
|
||||
适配器改用"与主关节直接相连、|multiplier| 最大的那个从动关节"反推开口
|
||||
（见 GripperLayout._pick_feedback / aperture_from_feedback）。
|
||||
|
||||
5) **仿真里的夹持靠不住，需要一把"抓取锁"**。仿真的夹持力不是设出来的，
|
||||
而是位置误差 × 接触刚度：规站着不动够用，机械臂一走连续轨迹（S01 快转
|
||||
150°、pre_lift 60~100mm）就在指间打滑 —— 实测工具长度 0.243m → 0.285m、
|
||||
规歪 62°，S07 随后拿到的几何全错。真机不会（46N 握力 + µ≈0.8 + 0.27kg）。
|
||||
所以本节点在**确认夹住之后**用 DetachableJoint 把物体固定到手指上，
|
||||
**指令要求张开时**解锁（详见 sim_grasp_lock.py；为什么不拿 is_gripped
|
||||
当解锁条件，见 :meth:`_sync_grasp_lock`）。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Float64MultiArray
|
||||
|
||||
from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from .sim_grasp_lock import GraspLockSpec, publish_lock_event
|
||||
from .sim_gripper import (
|
||||
DEFAULT_CONTROLLER,
|
||||
DEFAULT_CONTROLLERS_REL,
|
||||
DEFAULT_GRASP_TOLERANCE_MM,
|
||||
DEFAULT_MOVING_TOLERANCE_MM,
|
||||
GripperConfigError,
|
||||
GripperLayout,
|
||||
gripper_joint_order,
|
||||
is_grasping,
|
||||
layout_from_parts,
|
||||
parse_mimics,
|
||||
)
|
||||
|
||||
#: 主关节状态多少秒没更新就算"数据不新鲜"（仿真时间，不是墙钟）。
|
||||
#: 控制器 100Hz、JSB 也在这个量级，1 秒足够宽松。
|
||||
STATE_STALE_S = 1.0
|
||||
|
||||
#: 服务后缀 → GripperCommand.command 的映射。前四个就是 capabilities.py
|
||||
#: 里声明的接口名；execute 是通用入口（与 dual_gripper_driver 对齐）。
|
||||
_SERVICE_COMMANDS = ("open", "close", "move", "rotate")
|
||||
|
||||
|
||||
def _site_stroke_mm() -> Optional[float]:
|
||||
"""从 site.yaml 读真机夹爪行程（只为启动时打一句对比，读不到返回 None）。
|
||||
|
||||
读不到不影响工作 —— 它只是提示信息，不该让仿真起不来。
|
||||
"""
|
||||
try:
|
||||
from arm_interfaces import load_site # 延迟导入：只需要它一处
|
||||
except Exception:
|
||||
return None
|
||||
try:
|
||||
data = load_site()
|
||||
except Exception:
|
||||
return None
|
||||
grip = data.get("gripper") if isinstance(data, dict) else None
|
||||
if not isinstance(grip, dict):
|
||||
return None
|
||||
for key in ("stroke_mm", "stroke"):
|
||||
try:
|
||||
val = float(grip.get(key))
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
if val > 0:
|
||||
return val
|
||||
return None
|
||||
|
||||
|
||||
class SimGripperAdapter(Node):
|
||||
"""把 ``GripperExecute`` 服务翻译成 Gazebo 里 7 个夹爪关节的目标位置。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim_gripper_adapter")
|
||||
|
||||
# 参数：全部由 launch 从**同一份** prepare_urdf 结果与 controllers.yaml
|
||||
# 派生，避免这里自己再展开一次 xacro 造成两边不一致。
|
||||
self.declare_parameter("controllers_file", "")
|
||||
self.declare_parameter("controller", DEFAULT_CONTROLLER)
|
||||
self.declare_parameter("mimics", [""])
|
||||
# ⚠️ 不要 declare_parameter("use_sim_time", ...)：launch 会作为参数文件
|
||||
# 传进来，rclpy 那时已经自动声明过，再声明会抛异常让节点起不来。
|
||||
self.declare_parameter("namespace", "/gripper")
|
||||
self.declare_parameter("tolerance_mm", DEFAULT_GRASP_TOLERANCE_MM) # 夹住判据容差（毫米开口）
|
||||
self.declare_parameter("default_speed_pct", 50.0)
|
||||
self.declare_parameter("default_force_pct", 20.0)
|
||||
# 仿真抓取锁（见 sim_grasp_lock.py）。默认**关**：参数化之后，launch 才是
|
||||
# 唯一决定它开不开的地方 —— 单跑本节点做调试时不该莫名其妙去动 Gazebo。
|
||||
self.declare_parameter("grasp_lock_enabled", False)
|
||||
self.declare_parameter("grasp_lock_object", "gauge")
|
||||
self.declare_parameter("grasp_lock_attach_topic", "/sim/grasp_lock/attach")
|
||||
self.declare_parameter("grasp_lock_detach_topic", "/sim/grasp_lock/detach")
|
||||
self.declare_parameter("grasp_lock_timeout_ms", 3000)
|
||||
|
||||
controller = str(self.get_parameter("controller").value or DEFAULT_CONTROLLER)
|
||||
raw_ctrl = str(self.get_parameter("controllers_file").value or "")
|
||||
controllers_file = Path(raw_ctrl) if raw_ctrl else Path.cwd() / DEFAULT_CONTROLLERS_REL
|
||||
|
||||
# mimics 参数传进来的是字符串数组；空字符串要滤掉（ROS 对空数组不友好）。
|
||||
raw_mimics = [
|
||||
str(m).strip()
|
||||
for m in (self.get_parameter("mimics").value or [])
|
||||
if str(m).strip()
|
||||
]
|
||||
|
||||
urdf_xml = self._robot_description()
|
||||
try:
|
||||
order = gripper_joint_order(controllers_file, controller)
|
||||
relations = parse_mimics(raw_mimics)
|
||||
self.layout: GripperLayout = layout_from_parts(order, relations, urdf_xml)
|
||||
except GripperConfigError as exc:
|
||||
# 配置错就让节点直接失败：一个"什么都不动"的夹爪适配器比没有更糟 ——
|
||||
# 它看起来在工作，而上层会一直等一个永远不来的状态变化。
|
||||
self.get_logger().error(f"仿真夹爪配置有问题：\n{exc}")
|
||||
raise
|
||||
|
||||
self._controller = controller
|
||||
self._ns = str(self.get_parameter("namespace").value or "/gripper").rstrip("/")
|
||||
self._tolerance_mm = float(self.get_parameter("tolerance_mm").value)
|
||||
self._speed = float(self.get_parameter("default_speed_pct").value)
|
||||
self._force = float(self.get_parameter("default_force_pct").value)
|
||||
|
||||
# 当前目标（主关节值，米）与"最近一次是否有状态"。
|
||||
self._target = self.layout.aperture_to_source(self.layout.stroke_mm) # 默认全开
|
||||
self._feedback_state: Optional[float] = None
|
||||
self._state_stamp: Optional[float] = None
|
||||
# ⚠️ 反馈取 layout.feedback，**不是**主关节：上游的 finger_width_joint
|
||||
# 挂在孤立的 unused_finger_width_link 上，在 Gazebo 里根本不动
|
||||
# （实测：命令 0.100，状态恒为 0）。拿它当反馈会永远报"开口 0 mm"。
|
||||
# 详见 sim_gripper.GripperLayout._pick_feedback 的说明。
|
||||
self._state_name = f"{self.layout.feedback}"
|
||||
|
||||
self._pub_cmd = self.create_publisher(
|
||||
Float64MultiArray, f"/{controller}/commands", 10
|
||||
)
|
||||
self._pub_status = self.create_publisher(GripperStatus, f"{self._ns}/status", 10)
|
||||
self.create_subscription(JointState, "/joint_states", self._on_joint_state, 10)
|
||||
|
||||
self._srv: List[object] = []
|
||||
for name in _SERVICE_COMMANDS:
|
||||
self._srv.append(
|
||||
self.create_service(
|
||||
GripperExecute, f"{self._ns}/{name}", self._make_handler(name)
|
||||
)
|
||||
)
|
||||
self._srv.append(
|
||||
self.create_service(GripperExecute, f"{self._ns}/execute", self._on_execute)
|
||||
)
|
||||
|
||||
self.get_logger().info(f"夹爪：{self.layout.describe()}")
|
||||
if self.layout.feedback != self.layout.source:
|
||||
self.get_logger().info(
|
||||
f"夹爪开口反馈取自 {self.layout.feedback}（反推主关节 "
|
||||
f"{self.layout.source}；主关节本身在 Gazebo 里不跟随命令）"
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"夹爪行程口径：{self.layout.stroke_note(_site_stroke_mm())}"
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"服务 {self._ns}/{{{'|'.join(_SERVICE_COMMANDS)}|execute}} → "
|
||||
f"话题 /{controller}/commands"
|
||||
)
|
||||
|
||||
# 先把夹爪摆到默认全开，免得机器人一出场就是捏着的状态。
|
||||
self._publish_target(self._target)
|
||||
|
||||
# 抓取锁：规格从参数来（launch 从 grasp_lock.yaml 派生），状态用三态：
|
||||
# None = 不知道 / False = 已解锁 / True = 已上锁
|
||||
# 之所以要 None 而不是 bool + 计数器：发布失败时要能**重试**，
|
||||
# 而"发布失败"恰恰表现为"发出去没人听"（话题名错、进程起不来），
|
||||
# 只有把"未确认"单独表示出来才可能重试。
|
||||
self._lock = GraspLockSpec(
|
||||
enabled=bool(self.get_parameter("grasp_lock_enabled").value),
|
||||
object=str(self.get_parameter("grasp_lock_object").value or "gauge"),
|
||||
object_link="",
|
||||
robot_model="",
|
||||
robot_link="",
|
||||
attach_topic=str(self.get_parameter("grasp_lock_attach_topic").value or ""),
|
||||
detach_topic=str(self.get_parameter("grasp_lock_detach_topic").value or ""),
|
||||
state_topic="",
|
||||
publish_timeout_ms=int(self.get_parameter("grasp_lock_timeout_ms").value),
|
||||
)
|
||||
self._locked: Optional[bool] = None
|
||||
if self._lock.enabled:
|
||||
self.get_logger().info(
|
||||
f"抓取锁已启用：夹住后把 {self._lock.object} 用固定关节锁到手指上，"
|
||||
f"张开时解锁（attach={self._lock.attach_topic}）。\n"
|
||||
" 它补的是 Gazebo 缺的那块物理（刚性接触没有静摩擦迟滞、也不模拟"
|
||||
"指垫贴合）——\n"
|
||||
" 真机 46N 握力 + µ≈0.8 本来就夹得住，仿真的位置控制夹持则会在"
|
||||
"连续运动时打滑。"
|
||||
)
|
||||
|
||||
self.create_timer(0.1, self._tick_status)
|
||||
|
||||
# --- 参数与描述 -------------------------------------------------------
|
||||
def _robot_description(self) -> str:
|
||||
"""取归一化后的 robot_description（launch 与 RSP 用的是同一份）。"""
|
||||
if not self.has_parameter("robot_description"):
|
||||
self.declare_parameter("robot_description", "")
|
||||
xml = str(self.get_parameter("robot_description").value or "")
|
||||
if not xml.strip():
|
||||
raise GripperConfigError(
|
||||
"没拿到 robot_description 参数，无法确定夹爪关节的限位。\n"
|
||||
" 正常由 gazebo_sim.launch.py 传入（它与发给 robot_state_publisher "
|
||||
"的是同一份归一化描述）。单独手跑本节点时要显式给：\n"
|
||||
" ros2 run arm_sim_gazebo sim_gripper_adapter "
|
||||
"--ros-args -p robot_description:=\"$(xacro ...)\""
|
||||
)
|
||||
return xml
|
||||
|
||||
# --- 下发 -------------------------------------------------------------
|
||||
def _publish_target(self, source_value: float) -> None:
|
||||
"""把一整组关节目标（主 + 从动）按顺序发出去。"""
|
||||
self._target = source_value
|
||||
msg = Float64MultiArray()
|
||||
msg.data = [float(v) for v in self.layout.command_vector_from_source(source_value)]
|
||||
self._pub_cmd.publish(msg)
|
||||
|
||||
def _move_to_aperture(self, aperture_mm: float) -> None:
|
||||
self._publish_target(self.layout.aperture_to_source(aperture_mm))
|
||||
|
||||
# --- 状态 -------------------------------------------------------------
|
||||
def _on_joint_state(self, msg: JointState) -> None:
|
||||
for name, pos in zip(msg.name, msg.position):
|
||||
if name == self._state_name:
|
||||
self._feedback_state = float(pos)
|
||||
self._state_stamp = self._now()
|
||||
return
|
||||
|
||||
def _now(self) -> float:
|
||||
return self.get_clock().now().nanoseconds * 1e-9
|
||||
|
||||
def _fresh_state(self) -> Optional[float]:
|
||||
"""反馈关节的当前值；太久没更新或从没收到过就返回 None。"""
|
||||
if self._feedback_state is None or self._state_stamp is None:
|
||||
return None
|
||||
if (self._now() - self._state_stamp) > STATE_STALE_S:
|
||||
return None
|
||||
return self._feedback_state
|
||||
|
||||
def _status_msg(self) -> GripperStatus:
|
||||
state = self._fresh_state()
|
||||
msg = GripperStatus()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
# frame_id 留空：真机那边填的是 "<side>_gripper"，而仿真里没有任何东西
|
||||
# 发布那个坐标系（TF 只来自 robot_state_publisher）。
|
||||
# 填一个查不到的 frame 比留空更糟 —— tf2 会一直报 frame 不存在。
|
||||
msg.header.frame_id = ""
|
||||
msg.name = "gn01_sim"
|
||||
msg.gripper_type = "parallel"
|
||||
msg.connected = state is not None
|
||||
msg.initialized = state is not None
|
||||
msg.velocity = self._speed
|
||||
msg.force = self._force
|
||||
|
||||
if state is None:
|
||||
# 没状态就别编：position 保持 0 会让上层以为夹爪是合拢的。
|
||||
msg.state = "ERROR"
|
||||
msg.error_msg = (
|
||||
f"{STATE_STALE_S:.1f}s 内没收到 {self._state_name} 的 /joint_states，"
|
||||
f"无法反馈开口。检查 joint_state_broadcaster 是否 active。"
|
||||
)
|
||||
return msg
|
||||
|
||||
aperture = self.layout.aperture_from_feedback(state)
|
||||
target_aperture = self.layout.source_to_aperture(self._target)
|
||||
# 判"还在动"用**开口口径**（毫米），不用主关节值：开口↔关节值在 FK
|
||||
# 口径下是非线性的，拿米当口径的话同一个容差在不同开度下含义不同。
|
||||
moving = abs(aperture - target_aperture) > DEFAULT_MOVING_TOLERANCE_MM
|
||||
msg.position = float(aperture)
|
||||
# 真机这里填 DH 的千分比；仿真的"宽度关节"本身就是千分比语义
|
||||
#（0.100 m ≙ 1000‰），所以直接换算，口径与真机一致。
|
||||
msg.finger_position_per_mille = float(
|
||||
max(0.0, min(1000.0, aperture / max(1e-9, self.layout.stroke_mm) * 1000.0))
|
||||
)
|
||||
msg.is_moving = bool(moving)
|
||||
# 判据：指令在合拢侧 + 实际开口比指令大出容差（被工件挡住）。
|
||||
msg.is_gripped = is_grasping(
|
||||
target_aperture, aperture, self._tolerance_mm
|
||||
)
|
||||
if msg.is_gripped:
|
||||
msg.state = "GRIPPING"
|
||||
elif moving:
|
||||
msg.state = "MOVING"
|
||||
else:
|
||||
msg.state = "IDLE"
|
||||
return msg
|
||||
|
||||
def _tick_status(self) -> None:
|
||||
msg = self._status_msg()
|
||||
self._pub_status.publish(msg)
|
||||
self._sync_grasp_lock(bool(msg.is_gripped))
|
||||
|
||||
# --- 抓取锁（仿真专用）-------------------------------------------------
|
||||
def _sync_grasp_lock(self, gripped: bool) -> None:
|
||||
"""夹持状态 → 抓取锁。只在"确认夹住"时上锁，绝不自动解锁。
|
||||
|
||||
**为什么不在这里因为"判到没夹住"而解锁**：
|
||||
本适配器的 ``is_gripped`` 是**推断值**（指令到合拢位 + 实际开口被顶住），
|
||||
抓取锁要解决的恰恰是"规在指间打了滑"的情形 —— 打滑会让触觉接触
|
||||
闪断、``is_gripped`` 跟着抖。若拿它当解锁条件，就会得到
|
||||
"一抖就解锁 → 规真掉下去 → 更读不到夹持"的自毁循环。
|
||||
解锁只认**指令**（open / 开口变大的 move），那才是松手的真实原因。
|
||||
"""
|
||||
spec = self._lock
|
||||
if not spec.enabled or not gripped or self._locked is True:
|
||||
return
|
||||
if publish_lock_event(spec, True, logger=self.get_logger()):
|
||||
self._locked = True
|
||||
self.get_logger().info(
|
||||
f"抓取锁：已把 {spec.object} 锁到手指上（此后机械臂怎么动都不会滑）。"
|
||||
)
|
||||
else:
|
||||
# 失败不抛异常（锁只是补救措施），但要留痕：现象是"规掉了"而看不到真因。
|
||||
self._locked = None
|
||||
|
||||
def _release_grasp_lock(self, why: str) -> None:
|
||||
"""松开抓取锁。只在**指令明确要张开**时调用。"""
|
||||
spec = self._lock
|
||||
if not spec.enabled or self._locked is False:
|
||||
return
|
||||
if publish_lock_event(spec, False, logger=self.get_logger()):
|
||||
self._locked = False
|
||||
self.get_logger().info(f"抓取锁：已解锁（{why}）")
|
||||
else:
|
||||
self._locked = None
|
||||
|
||||
# --- 服务 -------------------------------------------------------------
|
||||
def _make_handler(self, verb: str):
|
||||
def handler(request: GripperExecute.Request, response: GripperExecute.Response):
|
||||
# 服务名本身就是命令；但请求里也可能带 command（通用入口风格）。
|
||||
# 若两者冲突，以**请求里的**为准并记一条 info —— 免得上层以为
|
||||
# 服务名能覆盖请求内容。
|
||||
req_verb = str(getattr(request.command, "command", "") or "").strip()
|
||||
if req_verb and req_verb != verb:
|
||||
self.get_logger().info(
|
||||
f"{self._ns}/{verb} 收到 command={req_verb!r}，按请求执行。"
|
||||
)
|
||||
return self._execute_verb(req_verb, request, response)
|
||||
return self._execute_verb(verb, request, response)
|
||||
|
||||
return handler
|
||||
|
||||
def _on_execute(
|
||||
self, request: GripperExecute.Request, response: GripperExecute.Response
|
||||
):
|
||||
verb = str(getattr(request.command, "command", "") or "").strip()
|
||||
if not verb:
|
||||
return self._fail(response, "execute 需要 request.command.command（如 open/close/move）")
|
||||
return self._execute_verb(verb, request, response)
|
||||
|
||||
def _execute_verb(
|
||||
self,
|
||||
verb: str,
|
||||
request: GripperExecute.Request,
|
||||
response: GripperExecute.Response,
|
||||
):
|
||||
cmd = request.command
|
||||
try:
|
||||
if verb == "open":
|
||||
self._release_grasp_lock("收到 open 指令")
|
||||
self._move_to_aperture(self.layout.stroke_mm)
|
||||
note = f"全开（{self.layout.stroke_mm:.1f} mm）"
|
||||
elif verb == "close":
|
||||
self._move_to_aperture(0.0)
|
||||
note = "全闭合（开口 0 mm）"
|
||||
elif verb == "move":
|
||||
want = float(cmd.position)
|
||||
clamped = self.layout.is_clamped(want)
|
||||
# 「开口变大」才叫松手。判据用**实测开口**而不是上一次指令：
|
||||
# 指令可以是"先夹到 30 再挪到 32"这种，比指令会误判成开爪。
|
||||
state = self._fresh_state()
|
||||
now_mm = (
|
||||
self.layout.aperture_from_feedback(state)
|
||||
if state is not None
|
||||
else None
|
||||
)
|
||||
if now_mm is not None and want > now_mm + self._tolerance_mm:
|
||||
self._release_grasp_lock(f"开口 {now_mm:.1f} → {want:.1f} mm")
|
||||
self._move_to_aperture(want)
|
||||
note = f"开口 → {want:.2f} mm"
|
||||
if clamped:
|
||||
note += (
|
||||
f"（**被限位截断**到 "
|
||||
f"{min(max(want, 0.0), self.layout.stroke_mm):.2f} mm）"
|
||||
)
|
||||
elif verb == "rotate":
|
||||
return self._fail(
|
||||
response,
|
||||
"仿真夹爪（GN01）是平行夹爪，没有旋转自由度；rotate 只在真机 RGI 上有效。",
|
||||
)
|
||||
elif verb == "stop":
|
||||
# 停在当前位置：把"当前实际值"当目标发回去。
|
||||
state = self._fresh_state()
|
||||
if state is None:
|
||||
return self._fail(response, "没有关节状态，stop 无法保持位置")
|
||||
# state 是**反馈关节**的值，得先换算回主关节口径再下发。
|
||||
self._publish_target(self.layout.source_from_feedback(state))
|
||||
note = (
|
||||
"保持当前位置（"
|
||||
f"{self.layout.aperture_from_feedback(state):.2f} mm）"
|
||||
)
|
||||
else:
|
||||
return self._fail(
|
||||
response,
|
||||
f"仿真夹爪不支持命令 {verb!r}；支持 "
|
||||
f"{list(_SERVICE_COMMANDS) + ['stop']}。",
|
||||
)
|
||||
except GripperConfigError as exc: # pragma: no cover - 构造期已挡住
|
||||
return self._fail(response, str(exc))
|
||||
|
||||
response.success = True
|
||||
response.message = note
|
||||
response.status = self._status_msg()
|
||||
self.get_logger().info(f"{self._ns}/{verb}：{note}")
|
||||
return response
|
||||
|
||||
def _fail(self, response: GripperExecute.Response, why: str):
|
||||
response.success = False
|
||||
response.message = why
|
||||
# status 仍然填：调用方拿到 success=false 时也需要知道当前开口，
|
||||
# 否则只能靠猜（真机的返回结构也是这个语义）。
|
||||
response.status = self._status_msg()
|
||||
self.get_logger().warn(f"{self._ns}：{why}")
|
||||
return response
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = None
|
||||
try:
|
||||
node = SimGripperAdapter()
|
||||
rclpy.spin(node)
|
||||
except GripperConfigError:
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(2)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+368
@@ -0,0 +1,368 @@
|
||||
"""由 URDF 做夹爪正运动学，把"开口毫米"换算成**真的**关节角（纯逻辑，不起 ROS）。
|
||||
|
||||
=============================================================================
|
||||
为什么需要它：上游的 <mimic> 是线性近似
|
||||
=============================================================================
|
||||
GN01 的描述用一条线性耦合把 6 个从动关节挂在宽度关节上：
|
||||
|
||||
从动关节 = 9.404 × 主关节 − 0.155
|
||||
|
||||
系数 9.404 是按**两端**拟合出来的，所以两端准、中段不准 —— 因为真实的
|
||||
手指不是"平移"，而是"绕着 knuckle 转 + 指掌再转"，指垫的间距
|
||||
``|tip_L(θ) − tip_R(θ)|`` 是 θ 的非线性函数。
|
||||
|
||||
实测（2026-09-24，Gazebo Fortress，见 test_sim_gripper_kin.py 里的黄金表）：
|
||||
|
||||
命令开口 指尖间距实测 与命令之差
|
||||
0 mm 27.09 mm （固有间距，指垫此时接触）
|
||||
8 mm 35.76 mm +0.67 mm
|
||||
20 mm 48.84 mm +1.75 mm
|
||||
50 mm 80.96 mm +3.87 mm
|
||||
100 mm 127.02 mm −0.07 mm
|
||||
|
||||
也就是说：**命令"开 20 mm"，两指实际相距 21.75 mm**。抓一个 20 mm 宽的
|
||||
通止规时，指垫之间会空出 1.75 mm —— 没有接触、没有接触力，触觉那一步
|
||||
在仿真里直接失效。这和真机（DH，毫米制、开多少就是多少）的口径不一致，
|
||||
仿真就失去了"拿真几何试算法"的意义。
|
||||
|
||||
所以本模块用 URDF 自己算正运动学，把 mm 反解成关节角：
|
||||
|
||||
aperture_to_source(20.0) → 令 |tip_L − tip_R| − 闭合间距 == 20.0 的关节值
|
||||
|
||||
这样"开口 20 mm"在仿真里就是**物理上真的 20 mm**，与真机口径一致。
|
||||
|
||||
=============================================================================
|
||||
为什么可靠
|
||||
=============================================================================
|
||||
本文件的正运动学与 Gazebo（DART 物理 + sdformat 转换后的模型）在
|
||||
0~100 mm 全程 **吻合到 0.01 mm**（上面那张表就是两边对比的结果）。
|
||||
两者完全独立：一个是我们的矩阵乘法，一个是物理引擎。这个一致性同时证明：
|
||||
|
||||
- URDF → SDF 的转换没有丢掉这些 link/joint 的偏移；
|
||||
- 我们对耦合公式的理解与 Gazebo 实际执行的一致。
|
||||
|
||||
=============================================================================
|
||||
通用性（换夹爪时）
|
||||
=============================================================================
|
||||
本模块不写死 GN01 的名字：
|
||||
- 树结构完全从 URDF 里读；
|
||||
- 两个"指尖"link 按命名约定找（``*left*finger*tip*`` / ``*right*...``），
|
||||
找不到就让调用方退回线性近似（并在日志里**明确说出口径变了**）；
|
||||
- 计算指尖间距用的参考系取两者的**最近公共祖先**，不需要知道夹爪 base
|
||||
叫什么名字（GN01 是 ``grav_base_link``，别的夹爪叫别的）。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import re
|
||||
import xml.etree.ElementTree as ET
|
||||
from dataclasses import dataclass
|
||||
from typing import Callable, Dict, List, Mapping, Optional, Sequence, Tuple
|
||||
|
||||
Mat3 = Tuple[Tuple[float, float, float], ...]
|
||||
Vec3 = Tuple[float, float, float]
|
||||
|
||||
|
||||
class KinematicsError(ValueError):
|
||||
"""URDF 结构与预期不符（缺 link、链断开、不是树等）。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointNode:
|
||||
"""URDF 里的一条 joint（父 → 子），带固定偏移与转动轴。"""
|
||||
|
||||
name: str
|
||||
type: str
|
||||
parent: str
|
||||
child: str
|
||||
xyz: Vec3
|
||||
rpy: Vec3
|
||||
axis: Vec3
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 3×3 矩阵小工具（只有旋转，位置单独带着走 —— 比 4×4 好读）
|
||||
# ---------------------------------------------------------------------------
|
||||
def _rot_rpy(rpy: Vec3) -> Mat3:
|
||||
r, p, y = rpy
|
||||
cr, sr = math.cos(r), math.sin(r)
|
||||
cp, sp = math.cos(p), math.sin(p)
|
||||
cy, sy = math.cos(y), math.sin(y)
|
||||
return (
|
||||
(cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr),
|
||||
(sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr),
|
||||
(-sp, cp * sr, cp * cr),
|
||||
)
|
||||
|
||||
|
||||
def _rot_axis(axis: Vec3, angle: float) -> Mat3:
|
||||
x, y, z = axis
|
||||
norm = math.sqrt(x * x + y * y + z * z)
|
||||
if norm < 1e-12:
|
||||
raise KinematicsError("关节 axis 是零向量，无法确定转动轴")
|
||||
x, y, z = x / norm, y / norm, z / norm
|
||||
c, s, k = math.cos(angle), math.sin(angle), 1.0 - math.cos(angle)
|
||||
return (
|
||||
(x * x * k + c, x * y * k - z * s, x * z * k + y * s),
|
||||
(y * x * k + z * s, y * y * k + c, y * z * k - x * s),
|
||||
(z * x * k - y * s, z * y * k + x * s, z * z * k + c),
|
||||
)
|
||||
|
||||
|
||||
def _mat_mul(a: Mat3, b: Mat3) -> Mat3:
|
||||
return tuple(
|
||||
tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3))
|
||||
for i in range(3)
|
||||
)
|
||||
|
||||
|
||||
def _mat_vec(a: Mat3, v: Vec3) -> Vec3:
|
||||
return tuple(sum(a[i][k] * v[k] for k in range(3)) for i in range(3)) # type: ignore[return-value]
|
||||
|
||||
|
||||
def _dist(a: Vec3, b: Vec3) -> float:
|
||||
return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# URDF 树
|
||||
# ---------------------------------------------------------------------------
|
||||
def _floats(text: Optional[str], default: Sequence[float]) -> Vec3:
|
||||
if not text:
|
||||
return tuple(default) # type: ignore[return-value]
|
||||
parts = text.split()
|
||||
if len(parts) != 3:
|
||||
raise KinematicsError(f"URDF 里的三元组长度不对: {text!r}")
|
||||
return (float(parts[0]), float(parts[1]), float(parts[2]))
|
||||
|
||||
|
||||
class UrdfTree:
|
||||
"""URDF 的 joint 树；只关心运动学（不碰 inertial / visual / collision）。"""
|
||||
|
||||
def __init__(self, xml: str) -> None:
|
||||
root = ET.fromstring(xml)
|
||||
self.by_child: Dict[str, JointNode] = {}
|
||||
self.parent_of: Dict[str, str] = {}
|
||||
for j in root.findall("joint"):
|
||||
origin = j.find("origin")
|
||||
axis_el = j.find("axis")
|
||||
parent_el, child_el = j.find("parent"), j.find("child")
|
||||
if parent_el is None or child_el is None or j.get("name") is None:
|
||||
raise KinematicsError("URDF 里有缺 name/parent/child 的 joint")
|
||||
node = JointNode(
|
||||
name=j.get("name", ""),
|
||||
type=j.get("type", "fixed"),
|
||||
parent=parent_el.get("link", ""),
|
||||
child=child_el.get("link", ""),
|
||||
xyz=_floats(origin.get("xyz") if origin is not None else None, (0, 0, 0)),
|
||||
rpy=_floats(origin.get("rpy") if origin is not None else None, (0, 0, 0)),
|
||||
axis=_floats(axis_el.get("xyz") if axis_el is not None else None, (1, 0, 0)),
|
||||
)
|
||||
if node.child in self.by_child:
|
||||
raise KinematicsError(f"link {node.child!r} 有两条父关节，不是树")
|
||||
self.by_child[node.child] = node
|
||||
self.parent_of[node.child] = node.parent
|
||||
|
||||
# --- 结构查询 ---------------------------------------------------------
|
||||
def ancestors(self, link: str) -> List[str]:
|
||||
"""从 link 一路向上到根（含自身）。"""
|
||||
out: List[str] = []
|
||||
seen = set()
|
||||
cur = link
|
||||
while True:
|
||||
out.append(cur)
|
||||
if cur not in self.parent_of:
|
||||
return out
|
||||
if cur in seen:
|
||||
raise KinematicsError(f"URDF 的 link 链里有环（{cur}）")
|
||||
seen.add(cur)
|
||||
cur = self.parent_of[cur]
|
||||
|
||||
def common_ancestor(self, a: str, b: str) -> str:
|
||||
"""最近公共祖先 link（用 A 的祖先链去查 B 的，第一个命中的就是）。"""
|
||||
b_chain = set(self.ancestors(b))
|
||||
for link in self.ancestors(a):
|
||||
if link in b_chain:
|
||||
return link
|
||||
raise KinematicsError(f"{a!r} 与 {b!r} 没有公共祖先（URDF 断了？）")
|
||||
|
||||
def chain(self, link: str, root: str) -> List[JointNode]:
|
||||
"""root → link 的关节序列。"""
|
||||
out: List[JointNode] = []
|
||||
cur = link
|
||||
while cur != root:
|
||||
if cur not in self.by_child:
|
||||
raise KinematicsError(f"从 {link!r} 上溯不到 {root!r}")
|
||||
node = self.by_child[cur]
|
||||
out.append(node)
|
||||
cur = node.parent
|
||||
return list(reversed(out))
|
||||
|
||||
# --- 正运动学 ---------------------------------------------------------
|
||||
def position(self, link: str, root: str, angles: Mapping[str, float]) -> Vec3:
|
||||
"""link 原点在 root 坐标系里的位置。angles 缺省视为 0。"""
|
||||
rot: Mat3 = ((1.0, 0, 0), (0, 1.0, 0), (0, 0, 1.0))
|
||||
pos: Vec3 = (0.0, 0.0, 0.0)
|
||||
for node in self.chain(link, root):
|
||||
offset = _mat_vec(rot, node.xyz)
|
||||
pos = (pos[0] + offset[0], pos[1] + offset[1], pos[2] + offset[2])
|
||||
rot = _mat_mul(rot, _rot_rpy(node.rpy))
|
||||
if node.type != "fixed":
|
||||
rot = _mat_mul(rot, _rot_axis(node.axis, float(angles.get(node.name, 0.0))))
|
||||
return pos
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 找指尖 link（按命名约定，不写死 GN01）
|
||||
# ---------------------------------------------------------------------------
|
||||
_LEFT_TIP_PATTERNS = (
|
||||
r".*left.*finger.*tip$",
|
||||
r".*left.*tip$",
|
||||
r".*left.*tcp$",
|
||||
)
|
||||
_RIGHT_TIP_PATTERNS = (
|
||||
r".*right.*finger.*tip$",
|
||||
r".*right.*tip$",
|
||||
r".*right.*tcp$",
|
||||
)
|
||||
|
||||
|
||||
def _pick(links: Sequence[str], patterns: Sequence[str]) -> Optional[str]:
|
||||
for pat in patterns:
|
||||
rx = re.compile(pat) # 大小写敏感：URDF 里普遍是小写
|
||||
hits = sorted(link for link in links if rx.match(link))
|
||||
if hits:
|
||||
# 多个命中时取名字最长的（``sim_left_finger_tip`` 比 ``left_tip``
|
||||
# 更可能是真正的指垫），并保持确定性（已排序）。
|
||||
return max(hits, key=len)
|
||||
return None
|
||||
|
||||
|
||||
def find_tip_pair(tree: UrdfTree) -> Optional[Tuple[str, str]]:
|
||||
"""按命名约定找左右指垫 link；找不到返回 None（调用方退回线性近似）。"""
|
||||
links = sorted(set(tree.by_child))
|
||||
left = _pick(links, _LEFT_TIP_PATTERNS)
|
||||
right = _pick(links, _RIGHT_TIP_PATTERNS)
|
||||
if left is None or right is None or left == right:
|
||||
return None
|
||||
return (left, right)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 开口曲线：mm ↔ 关节值
|
||||
# ---------------------------------------------------------------------------
|
||||
class ApertureCurve:
|
||||
"""由正运动学给出的"关节值 → 两指间距(mm)"曲线，以及它的反解。
|
||||
|
||||
``joint_value`` 是**主关节**的值（GN01 就是宽度关节，单位米）。
|
||||
调用方通过 ``angles_of(joint_value)`` 告诉本类"主关节取这个值时，
|
||||
每个关节分别是多少" —— 耦合关系归调用方（sim_gripper.py）管，
|
||||
本类只管几何，这样换夹爪/换耦合方式都不用动几何代码。
|
||||
"""
|
||||
|
||||
#: 反解用的二分次数（40 次足够把 100 mm 行程收敛到 1e-9 mm 以内）。
|
||||
BISECT_ITERS = 60
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
tree: UrdfTree,
|
||||
tips: Tuple[str, str],
|
||||
angles_of: Callable[[float], Mapping[str, float]],
|
||||
lower: float,
|
||||
upper: float,
|
||||
) -> None:
|
||||
self.tree = tree
|
||||
self.tips = tips
|
||||
self.angles_of = angles_of
|
||||
self.lower = float(lower)
|
||||
self.upper = float(upper)
|
||||
self.root = tree.common_ancestor(tips[0], tips[1])
|
||||
# 闭合位置（主关节到下限）的指尖间距 —— 指垫此时贴在一起，
|
||||
# 所以"开口"一律相对它算，这样开口 0 就是"真的合拢"。
|
||||
self._closed = self._raw_separation(self.lower)
|
||||
self._stroke = self._raw_separation(self.upper) - self._closed
|
||||
# ⚠️ 这里不能只判 `stroke <= 0`。实测过一次：把右侧 knuckle 的符号写反时，
|
||||
# 两指**同步**摆动，间距始终 45.0000000000 mm —— 差值是 4e-13，
|
||||
# 一个裸的 `<= 0` 会放过它。而放过的后果是**静默失效**：开口曲线是
|
||||
# 水平的，`joint_value()` 的二分会一路收敛到下限，
|
||||
# 于是"开 20 mm"变成"不动"，且没有任何报错。
|
||||
# 所以阈值取"名义行程的 1%"：真实夹爪不可能只有这么点行程，
|
||||
# 而浮点噪声远小于它。
|
||||
nominal_mm = abs(self.upper - self.lower) * 1000.0
|
||||
min_stroke = max(1e-6, 0.01 * nominal_mm)
|
||||
if self._stroke <= min_stroke:
|
||||
raise KinematicsError(
|
||||
f"正运动学算出两指行程只有 {self._stroke:.6f} mm"
|
||||
f"（名义 {nominal_mm:.1f} mm）—— 耦合方向或 tip link 选错了："
|
||||
f"关节在动，但两指间距几乎不变"
|
||||
)
|
||||
self._check_monotonic()
|
||||
|
||||
# --- 基础量 -----------------------------------------------------------
|
||||
def _raw_separation(self, joint_value: float) -> float:
|
||||
angles = self.angles_of(joint_value)
|
||||
a = self.tree.position(self.tips[0], self.root, angles)
|
||||
b = self.tree.position(self.tips[1], self.root, angles)
|
||||
return 1000.0 * _dist(a, b)
|
||||
|
||||
def _check_monotonic(self, samples: int = 24) -> None:
|
||||
"""间距必须随主关节单调增 —— 否则反解会给出"看起来对但跳变"的结果。
|
||||
|
||||
真实夹爪（含 GN01）都满足；不满足说明耦合表填错了（比如某条
|
||||
multiplier 正负号写反），这时**必须报错**而不是继续，
|
||||
否则抓取时开口会忽大忽小。
|
||||
"""
|
||||
prev = None
|
||||
for i in range(samples + 1):
|
||||
v = self.lower + (self.upper - self.lower) * i / samples
|
||||
cur = self._raw_separation(v)
|
||||
if prev is not None and cur < prev - 1e-6:
|
||||
raise KinematicsError(
|
||||
f"指尖间距不随主关节单调变化（{v:.4f} 处 {cur:.3f} mm < "
|
||||
f"前一点 {prev:.3f} mm）—— 检查耦合表的正负号"
|
||||
)
|
||||
prev = cur
|
||||
|
||||
# --- 对外 -------------------------------------------------------------
|
||||
@property
|
||||
def closed_separation_mm(self) -> float:
|
||||
"""合拢时两侧 tip frame 之间的固有间距（GN01 约 27 mm）。
|
||||
|
||||
注意它不是开口：指垫此时贴在一起。验收脚本要拿它把"间距"换成
|
||||
"开口"，见 scripts/check_sim_gripper.py。
|
||||
"""
|
||||
return self._closed
|
||||
|
||||
@property
|
||||
def stroke_mm(self) -> float:
|
||||
"""两指的**实测**行程（毫米）。GN01 ≈ 99.93，而不是名义的 100.0。
|
||||
|
||||
这个差不是误差：上游把宽度关节的限位定在 0~0.100 m，而线性耦合
|
||||
让指尖实际只走了 99.93 mm。启动日志里报的是这个实测值 ——
|
||||
报名义值会让"全开时开口读数与几何对不上"变成一个查不出的矛盾。
|
||||
"""
|
||||
return self._stroke
|
||||
|
||||
def aperture_mm(self, joint_value: float) -> float:
|
||||
"""主关节值 → 开口（毫米）。
|
||||
|
||||
超出限位时按限位截断（位置控制器本来也会截断，这里提前对齐口径）。
|
||||
"""
|
||||
v = min(max(float(joint_value), self.lower), self.upper)
|
||||
return self._raw_separation(v) - self._closed
|
||||
|
||||
def joint_value(self, aperture_mm: float) -> float:
|
||||
"""开口（毫米）→ 主关节值。二分反解，超出行程按端点截断。"""
|
||||
want = min(max(float(aperture_mm), 0.0), self._stroke)
|
||||
lo, hi = self.lower, self.upper
|
||||
for _ in range(self.BISECT_ITERS):
|
||||
mid = 0.5 * (lo + hi)
|
||||
if self.aperture_mm(mid) < want:
|
||||
lo = mid
|
||||
else:
|
||||
hi = mid
|
||||
return 0.5 * (lo + hi)
|
||||
|
||||
def is_clamped(self, aperture_mm: float) -> bool:
|
||||
"""请求的开口是否超出这个夹爪能开的范围（用来提醒上层）。"""
|
||||
return not (0.0 <= float(aperture_mm) <= self._stroke)
|
||||
+125
@@ -0,0 +1,125 @@
|
||||
#!/usr/bin/env python3
|
||||
"""仿真里的 ``/home`` —— 把真机 Flexiv 驱动的**同名服务**补齐，契约一致。
|
||||
|
||||
为什么需要它
|
||||
------------
|
||||
真机上 ``/home``（``std_srvs/Trigger``）由 ``arm_driver_flexiv`` 提供，
|
||||
它的实现就是"切工具系（可选）+ 转发给 ``/set_named_pose``"。
|
||||
仿真里没有 Flexiv 驱动，于是通止规装配（S01 回 home、S11 复位）在 Gazebo 里
|
||||
第一步就过不了自检 —— 而 S01 本身在两种环境下**没有区别**。
|
||||
|
||||
所以这里的做法不是"让上层代码判断自己在不在仿真"，而是**补一份同样的契约**：
|
||||
``/home`` → ``/set_named_pose``（``arm_msgs/srv/SetNamedPose``）。
|
||||
|
||||
有意保留的差异（写在这里，免得有人以为两边完全一样）：
|
||||
|
||||
* 真机的 ``/home`` 会顺带切工具系（``switch_tool_on_home``）；
|
||||
仿真里没有工具坐标系的概念，**不切**（S00 也把 ``/switch_tool`` 列为选检）。
|
||||
* 真机 ``/home`` 转发时用的 ``home_pose`` / ``home_speed`` 来自 ``site.yaml``
|
||||
（``go_home.pose`` / ``go_home.speed``）；这里用同名参数，默认值与 site 一致，
|
||||
launch 不必再传。
|
||||
|
||||
用法（由 ``gazebo_sim.launch.py`` 拉起）::
|
||||
|
||||
ros2 service call /home std_srvs/srv/Trigger '{}'
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.callback_groups import ReentrantCallbackGroup
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from rclpy.node import Node
|
||||
from std_srvs.srv import Trigger
|
||||
|
||||
from arm_msgs.srv import SetNamedPose
|
||||
|
||||
|
||||
class SimHomeService(Node):
|
||||
"""``/home`` → ``/set_named_pose``。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim_home_service")
|
||||
# 与 site.yaml 的 go_home 段保持一致：仿真与真机用**同一份默认值**，
|
||||
# 免得"仿真里回零是 4 秒、真机是 1 秒"这种口径差异。
|
||||
self.declare_parameter("home_pose", "home")
|
||||
self.declare_parameter("home_speed", 0.25)
|
||||
self.declare_parameter("set_named_pose_service", "/set_named_pose")
|
||||
#: 等 ``/set_named_pose`` 出现的超时 [s]。仿真栈刚起来时它可能还没就绪，
|
||||
#: 直接失败会让人误以为"服务没接线"。
|
||||
self.declare_parameter("wait_service_s", 5.0)
|
||||
#: 等 `</set_named_pose` 回应的超时 [s]。回零是慢动作，别设太小。
|
||||
self.declare_parameter("call_timeout_s", 60.0)
|
||||
|
||||
self._pose = str(self.get_parameter("home_pose").value or "home")
|
||||
self._speed = float(self.get_parameter("home_speed").value or 0.25)
|
||||
self._timeout = float(self.get_parameter("call_timeout_s").value)
|
||||
# Reentrant + 多线程：本节点的回调里要**同步等**另一个服务的应答，
|
||||
# 非重入的互斥组会自锁（真机驱动里踩过，注释同样留着）。
|
||||
self._cb = ReentrantCallbackGroup()
|
||||
self._named = self.create_client(
|
||||
SetNamedPose,
|
||||
str(self.get_parameter("set_named_pose_service").value),
|
||||
callback_group=self._cb,
|
||||
)
|
||||
self.create_service(Trigger, "/home", self._on_home, callback_group=self._cb)
|
||||
self.get_logger().info(
|
||||
f"仿真 /home 就绪 → {self._named.srv_name} 位姿[{self._pose}] "
|
||||
f"速度 {self._speed}（真机由 Flexiv 驱动提供同名服务）"
|
||||
)
|
||||
|
||||
def _on_home(self, request: Trigger.Request, response: Trigger.Response):
|
||||
wait_s = float(self.get_parameter("wait_service_s").value)
|
||||
if not self._named.wait_for_service(timeout_sec=wait_s):
|
||||
response.success = False
|
||||
response.message = (
|
||||
f"{self._named.srv_name} 不在 —— 命名位姿服务没起来。"
|
||||
"仿真是 gazebo_sim.launch.py 里的 named_pose_server，"
|
||||
"真机是 arm_bringup/stack.launch.py"
|
||||
)
|
||||
self.get_logger().error(response.message)
|
||||
return response
|
||||
|
||||
req = SetNamedPose.Request()
|
||||
req.pose_name = self._pose
|
||||
req.velocity_scale = self._speed
|
||||
fut = self._named.call_async(req)
|
||||
deadline = time.monotonic() + self._timeout
|
||||
while not fut.done():
|
||||
if time.monotonic() > deadline:
|
||||
response.success = False
|
||||
response.message = f"回位姿[{self._pose}]跟丢：{self._timeout:.0f}s 内没结果"
|
||||
self.get_logger().error(response.message)
|
||||
return response
|
||||
# 这里是**回调线程**（MultiThreadedExecutor），可以 sleep；
|
||||
# 不能 spin（会与执行器抢同一节点的回调）。
|
||||
time.sleep(0.02)
|
||||
res = fut.result()
|
||||
response.success = bool(res.success)
|
||||
response.message = res.message or f"已回位姿[{self._pose}]"
|
||||
(self.get_logger().info if response.success else self.get_logger().error)(
|
||||
f"/home → {response.message}"
|
||||
)
|
||||
return response
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = SimHomeService()
|
||||
executor = MultiThreadedExecutor(num_threads=4)
|
||||
executor.add_node(node)
|
||||
try:
|
||||
executor.spin()
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
executor.shutdown()
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,973 @@
|
||||
"""读 `devices/sim/gazebo/config/scene.yaml`：场景里摆了什么、摆在哪。
|
||||
|
||||
=============================================================================
|
||||
这个模块解决的两个问题
|
||||
=============================================================================
|
||||
1) "模型在哪"这件事必须有个**单一数据源**。
|
||||
之前摆在台面上的东西（通止规标记）是直接写死在 `assembly.world` 里的，
|
||||
改一次位置要动 world 文件 —— 而 world 里还有物理参数、插件、眼在手外的相机，
|
||||
动它就有一堆无关风险。所以场景物体统一挪到 scene.yaml，
|
||||
由启动器在 spawn 机械臂之后再 spawn 它们。
|
||||
|
||||
2) 位姿写对了但**物理上讲不通**的情况，必须能自动查出来。
|
||||
最典型的三类，全都是"看起来正常、跑起来不对"：
|
||||
- 模型悬空（z 比台面高）→ 一松手就掉，表现为"工件自己会动"
|
||||
- 模型穿进台面（z 比台面低）→ 求解器把它顶出来，表现为"工件自己弹起来"
|
||||
- 两个模型重叠 → 互相弹开，表现为"工件被推到别处去了"
|
||||
所以本模块除了读配置，还提供 validate_scene()：
|
||||
用**从 SDF 里真解析出来的碰撞体包围盒**去核对位置，而不是信注释。
|
||||
|
||||
⚠️ 为什么一定要解析 SDF 而不信 scene.yaml 里写的尺寸：
|
||||
尺寸属于模型（model.sdf），位置属于场景（scene.yaml）。
|
||||
如果在 scene.yaml 里再抄一份尺寸，两者迟早不一致 ——
|
||||
而且是那种"改小了模型但场景校验还按老尺寸过"，最难发现。
|
||||
=============================================================================
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import xml.etree.ElementTree as ET
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
import yaml
|
||||
|
||||
SCENE_REL = "devices/sim/gazebo/config/scene.yaml"
|
||||
|
||||
# 浮点比较容差。1e-6 m = 1μm —— 远小于任何有物理意义的装配误差，
|
||||
# 所以"差 1μm 以内"一律算相等，避免把 sqrt/字符串解析的尾差报成错误。
|
||||
EPS = 1e-6
|
||||
|
||||
|
||||
class SceneError(RuntimeError):
|
||||
"""scene.yaml 读不了、或模型文件缺失/不合法 —— 属于"起不来"的错误。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SceneObject:
|
||||
"""场景里的一个物体。pose 是**世界系绝对坐标**，z 是物体原点（我们约定在底面中心）。"""
|
||||
|
||||
name: str
|
||||
model: str
|
||||
x: float
|
||||
y: float
|
||||
z: float
|
||||
yaw: float = 0.0
|
||||
note: str = ""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Bounds:
|
||||
"""轴对齐包围盒，用来核对摆放。"""
|
||||
|
||||
x_min: float
|
||||
x_max: float
|
||||
y_min: float
|
||||
y_max: float
|
||||
|
||||
def contains(self, other: "Bounds", tol: float = EPS) -> bool:
|
||||
return (
|
||||
other.x_min >= self.x_min - tol
|
||||
and other.x_max <= self.x_max + tol
|
||||
and other.y_min >= self.y_min - tol
|
||||
and other.y_max <= self.y_max + tol
|
||||
)
|
||||
|
||||
def overlaps(self, other: "Bounds") -> bool:
|
||||
return not (
|
||||
other.x_min >= self.x_max - EPS
|
||||
or other.x_max <= self.x_min + EPS
|
||||
or other.y_min >= self.y_max - EPS
|
||||
or other.y_max <= self.y_min + EPS
|
||||
)
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Operating:
|
||||
"""作业条件：校验"成像几何是否与真机可比"。"""
|
||||
|
||||
camera: str
|
||||
camera_to_part_m: float
|
||||
camera_offset_below_flange_m: float
|
||||
# 哪个物体是"工件"（相机到工件表面的距离以它的上表面为基准）
|
||||
part_object: str = "workpiece"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PoseInfo:
|
||||
"""Gazebo 世界模型位姿回读的桥接参数 + "物体被撞走"的判据。
|
||||
|
||||
为什么要有它：``perception.source=truth`` 是**纯几何**的 —— 它按本文件的
|
||||
名义位姿算孔位，**不去问 Gazebo 物体现在到底在哪**。物体一旦被碰走，
|
||||
"真值"就开始撒谎，而每一处现象都指向别的原因（详见 scene.yaml 里
|
||||
``pose_info`` 那段注释）。这个结构就是让仿真能把"名义 vs 实际"对上。
|
||||
"""
|
||||
|
||||
gz_topic: str
|
||||
ros_topic: str
|
||||
ros_type: str = "tf2_msgs/msg/TFMessage"
|
||||
gz_type: str = "gz.msgs.Pose_V"
|
||||
#: 部件相对名义位姿的平移超过它 [m] 就算"被撞走了"。
|
||||
drift_tol_m: float = 0.002
|
||||
#: 同上，姿态 [deg]。
|
||||
drift_tol_deg: float = 2.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class SceneSpec:
|
||||
"""scene.yaml 的解析结果。"""
|
||||
|
||||
models_dir: str
|
||||
table_top_z: float
|
||||
table_bounds: Bounds
|
||||
operating: Operating
|
||||
objects: List[SceneObject] = field(default_factory=list)
|
||||
# 特征名 → (下限, 上限) 像素门槛。没写 = 不设门槛（只报告不判定）。
|
||||
gates_px: Dict[str, Tuple[float, float]] = field(default_factory=dict)
|
||||
# 世界模型位姿回读。**没写 = 不做漂移监视**（老场景文件仍然能读）。
|
||||
pose_info: Optional[PoseInfo] = None
|
||||
source: Optional[Path] = None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 读配置
|
||||
# ---------------------------------------------------------------------------
|
||||
def repo_root() -> Path:
|
||||
"""仓库根。**实现只有一份**：``robot_description.repo_root()``。
|
||||
|
||||
以前这里是 ``Path(__file__).resolve().parents[3]`` —— 那在**源码树**里对，
|
||||
在**安装后**是错的：``install_sim/`` 底下是**拷贝**（不是软链），
|
||||
所以装出来的模块算出来的"仓库根"是 ``install_sim/arm_sim_gazebo/lib``，
|
||||
而 ``devices/`` 根本不在那儿。现象是节点一启动就报
|
||||
"找不到场景配置 .../install_sim/arm_sim_gazebo/lib/devices/sim/gazebo/config/scene.yaml"。
|
||||
|
||||
而**节点永远是从安装路径启动的** —— 也就是说 parents[N] 这种写法在
|
||||
"脚本里手工跑"时看着没问题，一到正式启动就坏，属于最容易被漏掉的一类。
|
||||
|
||||
``robot_description.repo_root()`` 处理了三种情形（``ROS2_WS`` 环境变量、
|
||||
标记文件向上查找、cwd 向上查找），并且找不到时会说清怎么修。
|
||||
"""
|
||||
from arm_sim_gazebo.robot_description import repo_root as _robust_root
|
||||
|
||||
return _robust_root()
|
||||
|
||||
|
||||
def load_scene(path: Optional[Path] = None, root: Optional[Path] = None) -> SceneSpec:
|
||||
"""读场景配置。
|
||||
|
||||
**文件缺失时抛错**，不像 sim_site 那样给默认值 —— 区别在于：
|
||||
site.yaml 是站点数据（含真实序列号，本来就不入库），缺了是正常的；
|
||||
而 scene.yaml 是**框架自带**的场景定义，缺了说明仓库不完整，
|
||||
这时候静默用默认值会让人对着一个空台面调半天。
|
||||
"""
|
||||
base = Path(root) if root is not None else repo_root()
|
||||
p = Path(path) if path is not None else base / SCENE_REL
|
||||
|
||||
if not p.is_file():
|
||||
raise SceneError(
|
||||
f"找不到场景配置 {p}。\n"
|
||||
f" 它是框架自带的（不是站点数据），正常应该随仓库一起存在。\n"
|
||||
f" 若是刚克隆下来的仓库，确认 {SCENE_REL} 在；"
|
||||
f"或先用 --scene 指定别的场景文件。"
|
||||
)
|
||||
|
||||
try:
|
||||
doc = yaml.safe_load(p.read_text(encoding="utf-8")) or {}
|
||||
except yaml.YAMLError as exc:
|
||||
raise SceneError(f"{p} 解析失败: {exc}") from exc
|
||||
|
||||
if not isinstance(doc, dict):
|
||||
raise SceneError(f"{p} 的顶层不是映射（键值对）。")
|
||||
|
||||
missing = [k for k in ("models_dir", "table_top_z", "table_bounds", "objects") if k not in doc]
|
||||
if missing:
|
||||
raise SceneError(
|
||||
f"{p} 缺少必填键 {missing}。\n"
|
||||
f" 最小可用的场景长这样：\n"
|
||||
f" models_dir: devices/sim/gazebo/models\n"
|
||||
f" table_top_z: 0.40\n"
|
||||
f" table_bounds: {{x: [0.15, 0.95], y: [-0.60, 0.60]}}\n"
|
||||
f" objects: []"
|
||||
)
|
||||
|
||||
tb = doc.get("table_bounds") or {}
|
||||
try:
|
||||
table = Bounds(
|
||||
x_min=float(tb["x"][0]),
|
||||
x_max=float(tb["x"][1]),
|
||||
y_min=float(tb["y"][0]),
|
||||
y_max=float(tb["y"][1]),
|
||||
)
|
||||
except (KeyError, IndexError, TypeError, ValueError) as exc:
|
||||
raise SceneError(
|
||||
f"{p} 的 table_bounds 写错了（{exc}）。期望形如 "
|
||||
f"table_bounds: {{x: [0.15, 0.95], y: [-0.60, 0.60]}}"
|
||||
) from exc
|
||||
|
||||
op_raw = doc.get("operating") or {}
|
||||
operating = Operating(
|
||||
camera=str(op_raw.get("camera") or "sim_d405").strip().strip("/"),
|
||||
camera_to_part_m=float(op_raw.get("camera_to_part_m") or 0.20),
|
||||
camera_offset_below_flange_m=float(op_raw.get("camera_offset_below_flange_m") or 0.0),
|
||||
part_object=str(op_raw.get("part_object") or "workpiece"),
|
||||
)
|
||||
|
||||
objects: List[SceneObject] = []
|
||||
raw_objs = doc.get("objects") or []
|
||||
if not isinstance(raw_objs, list):
|
||||
raise SceneError(f"{p} 的 objects 不是列表。每一项形如 "
|
||||
f"{{name: workpiece, model: workpiece, pose: {{x: 0.52, y: 0, z: 0.40, yaw: 0}}}}")
|
||||
for i, raw in enumerate(raw_objs):
|
||||
if not isinstance(raw, dict):
|
||||
raise SceneError(f"{p} 的 objects[{i}] 不是映射。")
|
||||
pose = raw.get("pose") or {}
|
||||
name = str(raw.get("name") or "").strip()
|
||||
model = str(raw.get("model") or "").strip()
|
||||
if not name or not model:
|
||||
raise SceneError(
|
||||
f"{p} 的 objects[{i}] 缺 name 或 model。"
|
||||
f"两者都要有：name 是进 Gazebo 后的实体名，model 是模型目录名。"
|
||||
)
|
||||
try:
|
||||
objects.append(
|
||||
SceneObject(
|
||||
name=name,
|
||||
model=model,
|
||||
x=float(pose.get("x", 0.0)),
|
||||
y=float(pose.get("y", 0.0)),
|
||||
z=float(pose.get("z", 0.0)),
|
||||
yaw=float(pose.get("yaw", 0.0)),
|
||||
note=str(raw.get("note") or "").strip(),
|
||||
)
|
||||
)
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise SceneError(f"{p} 的 objects[{i}]（{name}）的 pose 不是数字: {exc}") from exc
|
||||
|
||||
return SceneSpec(
|
||||
models_dir=str(doc["models_dir"]).strip(),
|
||||
table_top_z=float(doc["table_top_z"]),
|
||||
table_bounds=table,
|
||||
operating=operating,
|
||||
objects=objects,
|
||||
gates_px=_parse_gates(p, doc.get("gates_px")),
|
||||
pose_info=_parse_pose_info(p, doc.get("pose_info")),
|
||||
source=p,
|
||||
)
|
||||
|
||||
|
||||
def _parse_pose_info(path: Path, raw: object) -> Optional[PoseInfo]:
|
||||
"""解析 pose_info。**没写就返回 None**（= 不做漂移监视）。
|
||||
|
||||
与 gates_px 不同，这一段是**可选**的：老场景文件没有它也必须能读。
|
||||
但只要写了，就必须写全 —— 少了话题名会得到一个"订阅了不存在的话题、
|
||||
永远没有数据"的监视器，那比没有监视更糟（看起来"监视器没报警"）。
|
||||
"""
|
||||
if raw is None:
|
||||
return None
|
||||
if not isinstance(raw, dict):
|
||||
raise SceneError(
|
||||
f"{path} 的 pose_info 不是映射。期望形如：\n"
|
||||
f" pose_info:\n"
|
||||
f" gz_topic: /world/assembly/pose/info\n"
|
||||
f" ros_topic: /sim/world_pose_info"
|
||||
)
|
||||
missing = [k for k in ("gz_topic", "ros_topic") if not str(raw.get(k, "")).strip()]
|
||||
if missing:
|
||||
raise SceneError(
|
||||
f"{path} 的 pose_info 缺字段 {missing}。\n"
|
||||
f" 这两个话题名必须都写：gz_topic 是 Gazebo 侧的 "
|
||||
f"/world/<world>/pose/info，ros_topic 是桥过来之后的名字。"
|
||||
)
|
||||
try:
|
||||
tol_m = float(raw.get("drift_tol_m", 0.002))
|
||||
tol_deg = float(raw.get("drift_tol_deg", 2.0))
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise SceneError(f"{path} 的 pose_info 容差不是数字: {exc}") from exc
|
||||
return PoseInfo(
|
||||
gz_topic=str(raw["gz_topic"]).strip(),
|
||||
ros_topic=str(raw["ros_topic"]).strip(),
|
||||
ros_type=str(raw.get("ros_type") or "tf2_msgs/msg/TFMessage").strip(),
|
||||
gz_type=str(raw.get("gz_type") or "gz.msgs.Pose_V").strip(),
|
||||
drift_tol_m=tol_m,
|
||||
drift_tol_deg=tol_deg,
|
||||
)
|
||||
|
||||
|
||||
def _parse_gates(path: Path, raw: object) -> Dict[str, Tuple[float, float]]:
|
||||
"""解析 gates_px。写成 {名字: [下限, 上限]}。
|
||||
|
||||
单独抽出来是因为这里的错误信息要能指出"哪一项写错了、该怎么写" ——
|
||||
门槛写错会静默地把校验变成永远通过，那比没校验更糟。
|
||||
"""
|
||||
if raw is None:
|
||||
return {}
|
||||
if not isinstance(raw, dict):
|
||||
raise SceneError(
|
||||
f"{path} 的 gates_px 不是映射。期望形如：\n"
|
||||
f" gates_px:\n insert_ring_radius: [10, 30]"
|
||||
)
|
||||
out: Dict[str, Tuple[float, float]] = {}
|
||||
for key, val in raw.items():
|
||||
try:
|
||||
lo, hi = float(val[0]), float(val[1])
|
||||
except (TypeError, IndexError, ValueError) as exc:
|
||||
raise SceneError(
|
||||
f"{path} 的 gates_px.{key} 写错了（{exc}）。"
|
||||
f"期望 [下限, 上限]，例如 [10, 30]。"
|
||||
) from exc
|
||||
if lo > hi:
|
||||
raise SceneError(
|
||||
f"{path} 的 gates_px.{key} 是 [{lo}, {hi}]，下限比上限大 —— "
|
||||
f"这样任何值都会被判为不合格。检查是不是写反了。"
|
||||
)
|
||||
out[str(key)] = (lo, hi)
|
||||
return out
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 从模型文件里读出真实几何
|
||||
# ---------------------------------------------------------------------------
|
||||
def model_sdf_path(spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None) -> Path:
|
||||
"""模型 SDF 的绝对路径：<仓库根>/<models_dir>/<model>/model.sdf。"""
|
||||
base = Path(root) if root is not None else repo_root()
|
||||
md = Path(spec.models_dir)
|
||||
if not md.is_absolute():
|
||||
md = base / md
|
||||
return md / obj.model / "model.sdf"
|
||||
|
||||
|
||||
def _local_extent(sdf_path: Path) -> Optional[Tuple[float, float, float, float, float, float]]:
|
||||
"""把一个模型所有 <collision> 的几何合成一个局部 AABB。
|
||||
|
||||
返回 (x_min, x_max, y_min, y_max, z_min, z_max)；文件读不了/没有碰撞体返回 None。
|
||||
|
||||
只看 collision 不看 visual：碰撞体才是"物理上占多大地方"。
|
||||
（本框架的模型里两者刻意做成一致，但检验摆放时以碰撞体为准 ——
|
||||
视觉偏出去一点不影响物理，碰撞体偏出去就会把别的东西顶飞。）
|
||||
"""
|
||||
boxes = collision_boxes(sdf_path)
|
||||
if not boxes:
|
||||
return None
|
||||
return (
|
||||
min(b[0] for b in boxes), max(b[1] for b in boxes),
|
||||
min(b[2] for b in boxes), max(b[3] for b in boxes),
|
||||
min(b[4] for b in boxes), max(b[5] for b in boxes),
|
||||
)
|
||||
|
||||
|
||||
def collision_boxes(sdf_path: Path) -> List[Tuple[float, float, float, float, float, float]]:
|
||||
"""模型里每个 <collision> 的局部 AABB，逐条返回（不合并）。
|
||||
|
||||
⚠️ 为什么必须**逐条**保留、不能只留一个总包围盒：
|
||||
有些物体是"套在一起"的 —— 通止规立在规座的方孔里就是典型。
|
||||
用总包围盒判"会不会撞"会把这种**正常嵌套**判成重叠（规座的包围盒
|
||||
是 60×60，规的包围盒是 30×30，两者必然相交），于是要么误报，
|
||||
要么就得加一个"允许重叠"的白名单把检查废掉。
|
||||
逐条比才可以正确得出"规身与规座的四个方块都不相交"。
|
||||
"""
|
||||
try:
|
||||
tree = ET.parse(sdf_path)
|
||||
except (OSError, ET.ParseError):
|
||||
return []
|
||||
|
||||
out: List[Tuple[float, float, float, float, float, float]] = []
|
||||
for coll in tree.iter("collision"):
|
||||
# 碰撞体自身的 <pose>：x y z roll pitch yaw（旋转一概忽略）
|
||||
px = py = pz = 0.0
|
||||
pose_el = coll.find("pose")
|
||||
if pose_el is not None and pose_el.text:
|
||||
parts = pose_el.text.split()
|
||||
if len(parts) >= 3:
|
||||
try:
|
||||
px, py, pz = (float(v) for v in parts[:3])
|
||||
except ValueError:
|
||||
px = py = pz = 0.0
|
||||
|
||||
geom = coll.find("geometry")
|
||||
if geom is None:
|
||||
continue
|
||||
half = None # (hx, hy, hz)
|
||||
box = geom.find("box")
|
||||
cyl = geom.find("cylinder")
|
||||
sph = geom.find("sphere")
|
||||
if box is not None:
|
||||
size_el = box.find("size")
|
||||
if size_el is not None and size_el.text:
|
||||
try:
|
||||
sx, sy, sz = (float(v) for v in size_el.text.split()[:3])
|
||||
half = (sx / 2.0, sy / 2.0, sz / 2.0)
|
||||
except ValueError:
|
||||
half = None
|
||||
elif cyl is not None:
|
||||
r_el, l_el = cyl.find("radius"), cyl.find("length")
|
||||
try:
|
||||
r = float(r_el.text) if r_el is not None and r_el.text else 0.0
|
||||
ln = float(l_el.text) if l_el is not None and l_el.text else 0.0
|
||||
half = (r, r, ln / 2.0)
|
||||
except ValueError:
|
||||
half = None
|
||||
elif sph is not None:
|
||||
r_el = sph.find("radius")
|
||||
try:
|
||||
r = float(r_el.text) if r_el is not None and r_el.text else 0.0
|
||||
half = (r, r, r)
|
||||
except ValueError:
|
||||
half = None
|
||||
if half is None:
|
||||
continue
|
||||
out.append(
|
||||
(
|
||||
px - half[0], px + half[0],
|
||||
py - half[1], py + half[1],
|
||||
pz - half[2], pz + half[2],
|
||||
)
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def world_boxes(
|
||||
spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None
|
||||
) -> List[Tuple[float, float, float, float, float, float]]:
|
||||
"""物体的各个碰撞体在**世界系**的 AABB。
|
||||
|
||||
yaw 的处理：绕 Z 转 θ 的矩形，外接盒的半宽是 hx·|cosθ| + hy·|sinθ|，
|
||||
半高同理；Z 不受影响。90° 整数倍会得到精确值，斜摆也仍然正确。
|
||||
|
||||
⚠️ 必须用"**中心 + 半宽**"变换，不能对 min/max 取绝对值再对称化 ——
|
||||
局部 AABB 里已经含了碰撞体 pose 的偏移量（例如规座的 four 块在
|
||||
x=∓16.75mm 处），把它对称化会把偏移吃掉、盒子凭空变大
|
||||
（实测：规座的一侧块从 x∈[-30,-3.5]mm 变成 x∈[-30,+30]mm，
|
||||
于是判出"规座四块全都和规的轴相交"的假重叠）。
|
||||
"""
|
||||
import math
|
||||
|
||||
boxes = collision_boxes(model_sdf_path(spec, obj, root))
|
||||
c, s = math.cos(obj.yaw), math.sin(obj.yaw)
|
||||
ac, as_ = abs(c), abs(s)
|
||||
out: List[Tuple[float, float, float, float, float, float]] = []
|
||||
for b in boxes:
|
||||
cx, cy = (b[0] + b[1]) / 2.0, (b[2] + b[3]) / 2.0
|
||||
hx, hy = (b[1] - b[0]) / 2.0, (b[3] - b[2]) / 2.0
|
||||
# 中心绕模型原点转 yaw
|
||||
wx = obj.x + cx * c - cy * s
|
||||
wy = obj.y + cx * s + cy * c
|
||||
# 旋转后的外接盒半宽
|
||||
rx = hx * ac + hy * as_
|
||||
ry = hx * as_ + hy * ac
|
||||
out.append((wx - rx, wx + rx, wy - ry, wy + ry, obj.z + b[4], obj.z + b[5]))
|
||||
return out
|
||||
|
||||
|
||||
def _boxes_overlap(
|
||||
a: Tuple[float, float, float, float, float, float],
|
||||
b: Tuple[float, float, float, float, float, float],
|
||||
tol: float = 1e-4,
|
||||
) -> bool:
|
||||
"""两个 AABB 是否真的相交（三轴都要相交才算）。
|
||||
|
||||
容差 0.1mm：**恰好贴合不算相交**。这条很关键 ——
|
||||
"规的台肩正好坐在规座顶面上"是正常装配状态，不是碰撞，
|
||||
若把贴合当相交，这种模型永远过不了检查，检查就会被人关掉。
|
||||
"""
|
||||
return not (
|
||||
b[0] >= a[1] - tol
|
||||
or b[1] <= a[0] + tol
|
||||
or b[2] >= a[3] - tol
|
||||
or b[3] <= a[2] + tol
|
||||
or b[4] >= a[5] - tol
|
||||
or b[5] <= a[4] + tol
|
||||
)
|
||||
|
||||
|
||||
def overlapping_pairs(
|
||||
spec: SceneSpec, root: Optional[Path] = None
|
||||
) -> List[Tuple[str, str, str]]:
|
||||
"""逐**碰撞体**核对哪些物体的哪些块真的互相嵌入。
|
||||
|
||||
返回 (物体A, 物体B, 说明)。空 = 没有互相嵌入。
|
||||
"""
|
||||
per_obj = {o.name: world_boxes(spec, o, root) for o in spec.objects}
|
||||
out: List[Tuple[str, str, str]] = []
|
||||
names = [o.name for o in spec.objects]
|
||||
for i in range(len(names)):
|
||||
for j in range(i + 1, len(names)):
|
||||
a, b = per_obj[names[i]], per_obj[names[j]]
|
||||
for ka, ba in enumerate(a):
|
||||
for kb, bb in enumerate(b):
|
||||
if _boxes_overlap(ba, bb):
|
||||
out.append(
|
||||
(
|
||||
names[i],
|
||||
names[j],
|
||||
f"碰撞体 #{ka} 与 #{kb} 互相嵌入",
|
||||
)
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def footprint(
|
||||
spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None
|
||||
) -> Optional[Tuple[float, float, float, float]]:
|
||||
"""物体摆在场景里之后，在世界系 XY 平面上占的矩形 (x_min, x_max, y_min, y_max)。
|
||||
|
||||
实现为"各碰撞体世界 AABB 在 XY 上的并"。**不再**自己写一遍 yaw 换算 ——
|
||||
那样两处换算逻辑迟早不一致，而"哪个才是对的"很难判断。
|
||||
"""
|
||||
boxes = world_boxes(spec, obj, root)
|
||||
if not boxes:
|
||||
return None
|
||||
return (
|
||||
min(b[0] for b in boxes), max(b[1] for b in boxes),
|
||||
min(b[2] for b in boxes), max(b[3] for b in boxes),
|
||||
)
|
||||
|
||||
|
||||
def model_height(spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None) -> Optional[float]:
|
||||
"""模型自身的高度（从原点到最高点）。用于推算工件上表面高度。"""
|
||||
ext = _local_extent(model_sdf_path(spec, obj, root))
|
||||
return None if ext is None else ext[5]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 校验
|
||||
# ---------------------------------------------------------------------------
|
||||
def validate_scene(spec: SceneSpec, root: Optional[Path] = None) -> List[str]:
|
||||
"""核对场景能不能真的跑起来。返回人类可读的问题列表（空 = 没问题）。
|
||||
|
||||
分两类，但都返回同一种字符串（调用方按需要决定是拦还是只告警）：
|
||||
"起不来"的错误：模型文件缺失、名字重复
|
||||
"跑起来不对"的错误：悬空 / 穿模 / 出界 / 互相重叠 / 原点不在底面
|
||||
"""
|
||||
base = Path(root) if root is not None else repo_root()
|
||||
problems: List[str] = []
|
||||
|
||||
md = Path(spec.models_dir)
|
||||
if not md.is_absolute():
|
||||
md = base / md
|
||||
if not md.is_dir():
|
||||
return [
|
||||
f"模型目录不存在：{md}\n"
|
||||
f" scene.yaml 的 models_dir 写的是 {spec.models_dir!r}，相对路径按**仓库根**解析。"
|
||||
]
|
||||
|
||||
seen: Dict[str, SceneObject] = {}
|
||||
for obj in spec.objects:
|
||||
if obj.name in seen:
|
||||
problems.append(
|
||||
f"物体名重复：{obj.name!r}。Gazebo 里实体名必须唯一，"
|
||||
f"否则后 spawn 的会失败（或覆盖先前的），而日志未必明显。"
|
||||
)
|
||||
seen[obj.name] = obj
|
||||
|
||||
sdf = model_sdf_path(spec, obj, root)
|
||||
cfg = sdf.parent / "model.config"
|
||||
if not sdf.is_file():
|
||||
problems.append(
|
||||
f"物体 {obj.name}：模型文件不存在 {sdf}\n"
|
||||
f" 期望布局：{md}/<模型名>/model.sdf。"
|
||||
f"scene.yaml 里写的 model={obj.model!r} 就是那个 <模型名>。"
|
||||
)
|
||||
continue
|
||||
if not cfg.is_file():
|
||||
problems.append(
|
||||
f"物体 {obj.name}：缺少 {cfg.name}（{cfg}）。\n"
|
||||
f" 没有它，model:// 引用会静默找不到（Gazebo 不报错）。"
|
||||
f"内容照 models/workpiece/model.config 抄一份，"
|
||||
f"<name> 必须等于目录名 {obj.model!r}。"
|
||||
)
|
||||
|
||||
ext = _local_extent(sdf)
|
||||
if ext is None:
|
||||
problems.append(
|
||||
f"物体 {obj.name}：{sdf.name} 里解析不出任何 <collision> 几何。\n"
|
||||
f" 没有碰撞体 = 物理上不存在：会穿过台面掉下去，而视觉上看着还在。"
|
||||
)
|
||||
continue
|
||||
|
||||
# 1) 原点必须在底面 —— 这是本框架"z = 台面高度"约定的前提
|
||||
if abs(ext[4]) > 1e-4:
|
||||
problems.append(
|
||||
f"物体 {obj.name}：模型原点不在底面（局部 z_min = {ext[4]:.4f} m）。\n"
|
||||
f" 场景里 z 是按'物体原点'给的，原点不在底面就会整体悬空或陷入台面。\n"
|
||||
f" 修法：改模型（把原点挪到底面中心）或改 scene.yaml 的 z，"
|
||||
f"但推荐改模型 —— 约定统一后场景文件才读得懂。"
|
||||
)
|
||||
|
||||
# 2) 落在台面上，且不穿模
|
||||
if obj.z < spec.table_top_z - 1e-4:
|
||||
problems.append(
|
||||
f"物体 {obj.name}：z={obj.z} 低于台面顶面 {spec.table_top_z}，穿进台子了。\n"
|
||||
f" 求解器会把它顶出来 → 表现为'工件自己弹起来'。"
|
||||
)
|
||||
|
||||
# 3) 不能出界
|
||||
fp = footprint(spec, obj, root)
|
||||
if fp is not None:
|
||||
fb = Bounds(x_min=fp[0], x_max=fp[1], y_min=fp[2], y_max=fp[3])
|
||||
if not spec.table_bounds.contains(fb):
|
||||
problems.append(
|
||||
f"物体 {obj.name}：占位 x∈[{fb.x_min:.3f},{fb.x_max:.3f}] "
|
||||
f"y∈[{fb.y_min:.3f},{fb.y_max:.3f}] 超出台面 "
|
||||
f"x∈[{spec.table_bounds.x_min},{spec.table_bounds.x_max}] "
|
||||
f"y∈[{spec.table_bounds.y_min},{spec.table_bounds.y_max}]。\n"
|
||||
f" 悬在台外的部分会掉下去 / 让它翻倒。"
|
||||
)
|
||||
|
||||
# 4) 物体之间不能互相嵌入。
|
||||
# 逐碰撞体、三维判，而不是"总包围盒在 XY 上相交"——
|
||||
# 因为正常装配里本来就存在嵌套（规立在规座的方孔里），
|
||||
# 粗判会把正常嵌套报成碰撞，逼着人把这个检查关掉。见 collision_boxes 的说明。
|
||||
for a, b, why in overlapping_pairs(spec, root):
|
||||
problems.append(
|
||||
f"物体 {a} 与 {b} 互相嵌入（{why}）。\n"
|
||||
f" 两个刚体一开始就互相嵌入 → 求解器把它们弹开，"
|
||||
f"表现为'工件/规被推到别处、自己飞走'，而你会怀疑是控制或标定问题。\n"
|
||||
f" 注意：恰好贴合不算嵌入（例如规的台肩正好坐在规座顶面上），"
|
||||
f"会被放行；这里报的是真穿透。"
|
||||
)
|
||||
|
||||
return problems
|
||||
|
||||
|
||||
def spawn_commands(
|
||||
spec: SceneSpec, root: Optional[Path] = None
|
||||
) -> List[List[str]]:
|
||||
"""每个场景物体一条 `ros_gz_sim create` 的参数列表（不含可执行文件名）。
|
||||
|
||||
为什么用 create 服务而不是把模型写进 world：
|
||||
world 是"固定不动的场景"（地面、台子、支架相机），
|
||||
而这些是按 site 需要摆/换位的**物料**。用 create 的话，
|
||||
换一个工件不用重启 Gazebo，也不用动 world 文件（动它的风险面太大，
|
||||
见 assembly.world 里关于插件的长注释）。
|
||||
"""
|
||||
out: List[List[str]] = []
|
||||
for obj in spec.objects:
|
||||
sdf = model_sdf_path(spec, obj, root)
|
||||
out.append(
|
||||
[
|
||||
"-file", str(sdf),
|
||||
"-name", obj.name,
|
||||
"-x", f"{obj.x:.6f}",
|
||||
"-y", f"{obj.y:.6f}",
|
||||
"-z", f"{obj.z:.6f}",
|
||||
"-Y", f"{obj.yaw:.6f}",
|
||||
]
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 成像几何：把"该看见什么"变成可校验的数字
|
||||
# ---------------------------------------------------------------------------
|
||||
def px_per_m(fx_px: float, distance_m: float) -> float:
|
||||
"""每米对应多少像素（小孔成像，垂直光轴近似）。
|
||||
|
||||
已知焦距与距离时的尺度换算，孔检测的像素门槛全靠它换算成物理尺寸。
|
||||
"""
|
||||
if distance_m <= 0:
|
||||
raise ValueError(f"距离必须为正，收到 {distance_m}")
|
||||
return fx_px / distance_m
|
||||
|
||||
|
||||
def feature_pixels(
|
||||
fx_px: float, distance_m: float, features: Dict[str, float]
|
||||
) -> Dict[str, float]:
|
||||
"""把一组特征尺寸（米）换算成像素。返回同名的像素字典，便于直接打印。"""
|
||||
s = px_per_m(fx_px, distance_m)
|
||||
return {k: v * s for k, v in features.items()}
|
||||
|
||||
|
||||
def check_pixel_gates(
|
||||
px: Dict[str, float], gates: Dict[str, Tuple[float, float]]
|
||||
) -> List[str]:
|
||||
"""按像素门槛核对特征是否落在可检测区间内。返回问题列表。
|
||||
|
||||
为什么这件事要在仿真侧、而不是等真机：这些门槛（hough 半径、孔间距）
|
||||
都是**像素**量，而仿真里唯一能提前算出它们的就是几何。
|
||||
等上了真机才发现"孔间距只有 85px，低于门槛 90"要贵得多。
|
||||
"""
|
||||
out: List[str] = []
|
||||
for key, (lo, hi) in gates.items():
|
||||
if key not in px:
|
||||
continue
|
||||
v = px[key]
|
||||
if v < lo or v > hi:
|
||||
out.append(
|
||||
f"{key} = {v:.1f}px 落在可检测区间 [{lo}, {hi}] 之外。\n"
|
||||
f" 成因只有三种：相机到工件的距离不对 / 相机图幅不对 / 特征尺寸不对。\n"
|
||||
f" 分别对应：scene.yaml 的 camera_to_part_m、"
|
||||
f"cameras.yaml 的 width、模型里的特征尺寸。"
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def operating_flange_z(spec: SceneSpec, root: Optional[Path] = None) -> Optional[float]:
|
||||
"""按"相机到工件的距离"反推仿真里法兰应该悬停多高。
|
||||
|
||||
推导：工件上表面 = 台面 + 工件模型自身高度
|
||||
相机光心 = 法兰 - 伸出量（竖直朝下装配时，伸出量朝工件方向）
|
||||
法兰 = 工件上表面 + 相机到工件距离 + 伸出量
|
||||
|
||||
这个值**不等于**真机 site.yaml 的 hole_hover_z_m（0.23）——
|
||||
原因见 scene.yaml 里 operating 段的说明与 README 的"仿真 ≠ 真机"。
|
||||
"""
|
||||
part = next((o for o in spec.objects if o.name == spec.operating.part_object), None)
|
||||
if part is None:
|
||||
return None
|
||||
h = model_height(spec, part, root)
|
||||
if h is None:
|
||||
return None
|
||||
part_top = spec.table_top_z + h
|
||||
return part_top + spec.operating.camera_to_part_m + spec.operating.camera_offset_below_flange_m
|
||||
|
||||
|
||||
def object_pose_map(spec: SceneSpec) -> Dict[str, Tuple[float, float, float, float]]:
|
||||
"""name → (x, y, z, yaw)。启动日志与验收脚本都用它打印"应该摆在哪"。"""
|
||||
return {o.name: (o.x, o.y, o.z, o.yaw) for o in spec.objects}
|
||||
|
||||
|
||||
def parse_spawned_poses(text: str) -> Dict[str, Tuple[float, float, float]]:
|
||||
"""解析 `ign model --list` / `ign model -m <名>` 的输出，解出实体位姿。
|
||||
|
||||
⚠️ 这段解析**曾经是坏的，而且是静默坏的**：它按 "x: N / y: N / z: N"
|
||||
键值对来读，而 Fortress 的实际输出是
|
||||
|
||||
Model: [105]
|
||||
- Name: workpiece
|
||||
- Pose [ XYZ (m) ] [ RPY (rad) ]:
|
||||
[0.520000 -0.000000 0.400000]
|
||||
[0.000000 -0.000000 0.000000]
|
||||
|
||||
两者对不上 → 永远解析出空字典 → 调用方按设计"降级成跳过位姿核对" →
|
||||
位姿检查**从来没真正跑过**，而日志只说"跳过"，看起来像正常降级。
|
||||
教训：容器格式变了的时候，"宽容降级"会把"没检查"伪装成"检查通过"。
|
||||
两种格式现在都支持，且都有单测钉住（含真实的 Fortress 输出样本）。
|
||||
|
||||
仍然保留"解析不出来就返回空字典"的降级行为（Gazebo 的文本输出没有稳定
|
||||
契约），但调用方应当把"解析不出来"当作**告警**而不是静默通过 ——
|
||||
check_sim_scene.py 会打印"跳过"并说明原因。
|
||||
"""
|
||||
import re
|
||||
|
||||
out: Dict[str, Tuple[float, float, float]] = {}
|
||||
current: Optional[str] = None
|
||||
vals: Dict[str, float] = {}
|
||||
# 正在等 "Pose [...]:" 下面那一行方括号数值
|
||||
want_xyz = False
|
||||
|
||||
def _flush() -> None:
|
||||
if current and {"x", "y", "z"} <= set(vals):
|
||||
out[current] = (vals["x"], vals["y"], vals["z"])
|
||||
|
||||
for line in text.splitlines():
|
||||
m_name = re.search(r"^\s*(?:-\s*)?Name:\s*(\S+)", line)
|
||||
if m_name:
|
||||
_flush()
|
||||
current = m_name.group(1)
|
||||
vals = {}
|
||||
want_xyz = False
|
||||
continue
|
||||
|
||||
# `ign model --list` 的实体名单独一行，形如 " - workpiece"
|
||||
m_ent = re.match(r"^\s*-\s*(\S+)\s*$", line)
|
||||
if m_ent:
|
||||
_flush()
|
||||
current = m_ent.group(1)
|
||||
vals = {}
|
||||
want_xyz = False
|
||||
continue
|
||||
|
||||
if "Pose" in line and "[" in line and "]" in line:
|
||||
want_xyz = True
|
||||
continue
|
||||
|
||||
if want_xyz:
|
||||
nums = re.findall(r"-?\d+(?:\.\d*)?(?:[eE][-+]?\d+)?", line)
|
||||
if current and len(nums) >= 3:
|
||||
try:
|
||||
vals = {"x": float(nums[0]), "y": float(nums[1]), "z": float(nums[2])}
|
||||
except ValueError:
|
||||
vals = {}
|
||||
want_xyz = False
|
||||
continue
|
||||
|
||||
# 兼容旧式 / 其他工具的 "x: N" 键值对写法
|
||||
m = re.match(r"\s*([xyz]):\s*(-?[0-9.eE+]+)\s*$", line)
|
||||
if m and current:
|
||||
try:
|
||||
vals[m.group(1)] = float(m.group(2))
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
_flush()
|
||||
return out
|
||||
|
||||
|
||||
def pose_mismatches(
|
||||
spec: SceneSpec,
|
||||
actual: Dict[str, Tuple[float, float, float]],
|
||||
tol_m: float = 5e-3,
|
||||
) -> List[str]:
|
||||
"""把"场景里写的位姿"与"Gazebo 里的实际位姿"对比，返回不一致的条目。
|
||||
|
||||
容差 5mm 起步：物体一 spawn 就会在重力下微动（落座/回弹），
|
||||
卡到 1e-6 只会得到永远为真的告警。这里要抓的是
|
||||
"根本没 spawn 到那个位置"（差几十厘米）或"spawn 了但掉下去了"。
|
||||
"""
|
||||
out: List[str] = []
|
||||
for obj in spec.objects:
|
||||
got = actual.get(obj.name)
|
||||
if got is None:
|
||||
out.append(
|
||||
f"{obj.name}：Gazebo 里找不到这个实体（scene.yaml 里写了，但没 spawn 成功？）"
|
||||
)
|
||||
continue
|
||||
d = max(abs(got[0] - obj.x), abs(got[1] - obj.y), abs(got[2] - obj.z))
|
||||
if d > tol_m:
|
||||
out.append(
|
||||
f"{obj.name}：位姿与 scene.yaml 不符（最大偏差 {d * 1000:.1f}mm）。\n"
|
||||
f" 场景要求 ({obj.x:.3f}, {obj.y:.3f}, {obj.z:.3f})，"
|
||||
f"实际 ({got[0]:.3f}, {got[1]:.3f}, {got[2]:.3f})。\n"
|
||||
f" 偏差大 = 没摆到位置；Z 偏大且持续 = 悬空掉下来了。"
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SceneRing:
|
||||
"""工件上的一个嵌件亮环（模型里 ``<visual name="*_ring">`` 的几何）。
|
||||
|
||||
坐标是**物体局部系、单位米**（SDF 原生的量纲）：``z`` 是环心高度，
|
||||
``radius`` 是亮环外半径。用途有两个——投影成像素（要 x/y/z）和算像素半径
|
||||
（要 radius）。
|
||||
|
||||
为什么单独建一个类型而不是返回元组：下游要按下标取值，写错不会报错，
|
||||
只会安静地算偏 —— 而"孔位总偏一点"是最难归因的一类症状。
|
||||
"""
|
||||
|
||||
name: str
|
||||
x: float
|
||||
y: float
|
||||
z: float
|
||||
radius: float
|
||||
|
||||
|
||||
def _visual_geometry(tree: ET.ElementTree) -> Tuple[List[SceneRing], Dict[str, float]]:
|
||||
"""一次遍历解出（亮环列表, 孔半径表）。名字约定见 :func:`features_from_workpiece`。"""
|
||||
rings: List[SceneRing] = []
|
||||
bores: Dict[str, float] = {}
|
||||
for vis in tree.iter("visual"):
|
||||
name = vis.get("name") or ""
|
||||
cyl = vis.find("geometry/cylinder")
|
||||
if cyl is None:
|
||||
continue # 板面是 box，不是"环/孔" —— 不看
|
||||
r_el = cyl.find("radius")
|
||||
if r_el is None or not r_el.text:
|
||||
continue
|
||||
try:
|
||||
r = float(r_el.text)
|
||||
except ValueError:
|
||||
continue
|
||||
if name.endswith("_ring"):
|
||||
pose_el = vis.find("pose")
|
||||
if pose_el is None or not pose_el.text:
|
||||
# 没有 pose 就没有圆心。**宁可不收**，也不要拿 (0,0,0) 当圆心：
|
||||
# 那会让"两孔间距"算成 0，看起来像"检测不到孔"，白查半天。
|
||||
continue
|
||||
parts = pose_el.text.split()
|
||||
if len(parts) < 3:
|
||||
continue
|
||||
try:
|
||||
x, y, z = float(parts[0]), float(parts[1]), float(parts[2])
|
||||
except ValueError:
|
||||
continue
|
||||
rings.append(SceneRing(name=name, x=x, y=y, z=z, radius=r))
|
||||
elif name == "center_bore":
|
||||
# ⚠️ 必须先判 center_bore 再判 _bore 后缀：名字 "center_bore" 也以
|
||||
# "_bore" 结尾，先判后缀会让中心大孔的半径覆盖掉小孔的。
|
||||
# （踩过：insert_bore_radius 报出 0.02，看起来"孔比环大"，很费解。）
|
||||
bores["center_bore"] = r
|
||||
elif name.endswith("_bore"):
|
||||
bores["insert_bore"] = r
|
||||
return rings, bores
|
||||
|
||||
|
||||
def rings_local(
|
||||
spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None
|
||||
) -> List[SceneRing]:
|
||||
"""工件模型里所有嵌件亮环（物体局部系，单位米）。
|
||||
|
||||
与 :func:`features_from_workpiece` **共用同一份解析**（``_visual_geometry``），
|
||||
因为两处必须永远一致：前者算"该看到多大"，后者算"该看到在哪" ——
|
||||
漂移了就会出现"像素门槛说能看到、真值却投影不到那儿"的怪现象。
|
||||
"""
|
||||
try:
|
||||
tree = ET.parse(model_sdf_path(spec, obj, root))
|
||||
except (OSError, ET.ParseError):
|
||||
return []
|
||||
return _visual_geometry(tree)[0]
|
||||
|
||||
|
||||
def features_from_workpiece(
|
||||
spec: SceneSpec, obj: SceneObject, root: Optional[Path] = None
|
||||
) -> Dict[str, float]:
|
||||
"""从工件模型的视觉里读出"该被检测到的特征尺寸"（米）。
|
||||
|
||||
读的是 <visual> 而不是 <collision>：检测看的是图像，图像来自视觉几何。
|
||||
（工件的碰撞体是方孔、视觉是圆环，两者尺寸不同 —— 这不是笔误：
|
||||
碰撞体只要能让规穿过去，视觉才决定相机看到什么。见模型文件头的说明。）
|
||||
|
||||
识别的办法是按 visual 的**名字**取：
|
||||
insert_*_ring → 嵌件亮环（外径 = 2×radius）→ 会被 HoughCircles 检测
|
||||
insert_*_bore → 孔本身（可能低于 hough 下限，用于交叉印证）
|
||||
center_bore → 中心大孔（应当超过 hough 上限而被滤掉）
|
||||
名字是本框架的约定，写在模型的注释里；改名字要同步改这里。
|
||||
"""
|
||||
import math
|
||||
|
||||
try:
|
||||
tree = ET.parse(model_sdf_path(spec, obj, root))
|
||||
except (OSError, ET.ParseError):
|
||||
return {}
|
||||
|
||||
rings, bores = _visual_geometry(tree)
|
||||
out: Dict[str, float] = {}
|
||||
if rings:
|
||||
out["insert_ring_radius"] = rings[-1].radius
|
||||
if "center_bore" in bores:
|
||||
out["center_bore_radius"] = bores["center_bore"]
|
||||
if "insert_bore" in bores:
|
||||
out["insert_bore_radius"] = bores["insert_bore"]
|
||||
# 两孔间距 = 两个环心之间的距离
|
||||
if len(rings) == 2:
|
||||
out["hole_spacing"] = math.hypot(
|
||||
rings[0].x - rings[1].x, rings[0].y - rings[1].y
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def visual_ring_features(
|
||||
spec: SceneSpec, root: Optional[Path] = None
|
||||
) -> Dict[str, float]:
|
||||
"""把默认工件（operating.part_object）的检测特征整理成像素换算的输入。"""
|
||||
part = next((o for o in spec.objects if o.name == spec.operating.part_object), None)
|
||||
if part is None:
|
||||
return {}
|
||||
return features_from_workpiece(spec, part, root)
|
||||
|
||||
|
||||
def operating_pixel_report(
|
||||
spec: SceneSpec, fx_px: float, root: Optional[Path] = None
|
||||
) -> Dict[str, float]:
|
||||
"""在"作业条件"（operating 段声明的相机到工件距离）下，把工件的检测特征换算成像素。
|
||||
|
||||
这是把"仿真里到底看不看得见该看见的东西"变成**可打印、可断言**的数字：
|
||||
亮环半径 → 应当落在 hough 的 [min_radius, max_radius]
|
||||
两孔间距 → 应当落在 [hole_distance_min, hole_distance_max]
|
||||
中心大孔 → 应当**超出** max_radius（否则它会被误当成目标孔）
|
||||
"""
|
||||
feats = visual_ring_features(spec, root)
|
||||
if not feats:
|
||||
return {}
|
||||
return feature_pixels(fx_px, spec.operating.camera_to_part_m, feats)
|
||||
@@ -0,0 +1,220 @@
|
||||
"""读 `config/site.yaml` 的 `sim.*` 段。
|
||||
|
||||
=============================================================================
|
||||
为什么需要这个模块
|
||||
=============================================================================
|
||||
`config/site.yaml` 的 `sim` 段（world / physics_enable / joint_prefix）与
|
||||
`devices/sim/gazebo/manifest.yaml` 的 `config_schema` 一直宣称"改这里就生效"，
|
||||
但**没有任何代码读它**：world 路径硬编码在 robot_description._WORLD_REL 里。
|
||||
|
||||
于是产生最难受的一类 bug：用户按文档改了 `site.yaml`，启动后行为毫无变化，
|
||||
也不报错。既然文档与 manifest 都把它当契约，就必须真的读它。
|
||||
|
||||
要读，但也不能假装所有键都接线了 —— 有些键（physics_enable / joint_prefix）
|
||||
目前确实还没接。所以本模块除了读值，还提供 unwired_warnings()：
|
||||
把"声明了、但当前实现并不消费"的差异显式报出来，
|
||||
把静默无效变成一条看得见的告警。等哪天真的接线了，告警自然消失。
|
||||
=============================================================================
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import yaml
|
||||
|
||||
SITE_REL = "config/site.yaml"
|
||||
SITE_EXAMPLE_REL = "config/site.example.yaml"
|
||||
|
||||
# 与 manifest.yaml 的 config_schema 默认值保持一致。
|
||||
# 三处（site.yaml 注释 / manifest / 这里）都写了默认值，改动时必须同步 ——
|
||||
# 所以下面有用例盯着这里和 manifest 的一致性。
|
||||
DEFAULT_WORLD = ""
|
||||
DEFAULT_PHYSICS_ENABLE = True
|
||||
DEFAULT_JOINT_PREFIX = "sim_"
|
||||
|
||||
|
||||
class SimSiteError(RuntimeError):
|
||||
"""site.yaml 读不了或内容不合法。"""
|
||||
|
||||
|
||||
@dataclass
|
||||
class SimSite:
|
||||
"""`sim.*` 段的解析结果。"""
|
||||
|
||||
world: str = DEFAULT_WORLD
|
||||
physics_enable: bool = DEFAULT_PHYSICS_ENABLE
|
||||
joint_prefix: str = DEFAULT_JOINT_PREFIX
|
||||
# 这些键在 site.yaml 里**显式**出现过吗？
|
||||
# 用来区分"用户说了要关物理"和"用户没提，所以用默认值"——
|
||||
# 前者值得告警，后者没啥可说。
|
||||
present: set = field(default_factory=set)
|
||||
source: Optional[Path] = None
|
||||
|
||||
|
||||
def load_sim_site(root: Optional[Path] = None) -> SimSite:
|
||||
"""读 `config/site.yaml` 的 sim 段。文件不存在则返回全默认值。
|
||||
|
||||
故意**不**在文件缺失时报错：仿真要能在没有站点的机器上跑起来
|
||||
（site.yaml 里含真实设备 ID，本来就不该提交，见 README 的"要提交什么"）。
|
||||
"""
|
||||
base = Path(root) if root is not None else repo_root()
|
||||
path = base / SITE_REL
|
||||
|
||||
if not path.is_file():
|
||||
return SimSite()
|
||||
|
||||
try:
|
||||
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except yaml.YAMLError as exc:
|
||||
raise SimSiteError(
|
||||
f"{path} 解析失败: {exc}\n"
|
||||
f" 这一行会让仿真启动不了。修好它，或临时改名为 site.yaml.bak 忽略。"
|
||||
) from exc
|
||||
|
||||
if not isinstance(doc, dict):
|
||||
raise SimSiteError(f"{path} 的顶层不是映射（键值对）。")
|
||||
|
||||
raw = doc.get("sim")
|
||||
if raw is None:
|
||||
return SimSite(source=path)
|
||||
if not isinstance(raw, dict):
|
||||
raise SimSiteError(
|
||||
f"{path} 的 sim 段不是映射（键值对），而是 {type(raw).__name__}。"
|
||||
f"它应该像这样：\n sim:\n world: devices/sim/gazebo/worlds/assembly.world"
|
||||
)
|
||||
|
||||
site = SimSite(source=path)
|
||||
if "world" in raw:
|
||||
site.world = str(raw.get("world") or "").strip()
|
||||
site.present.add("world")
|
||||
if "physics_enable" in raw:
|
||||
site.physics_enable = bool(raw.get("physics_enable"))
|
||||
site.present.add("physics_enable")
|
||||
if "joint_prefix" in raw:
|
||||
site.joint_prefix = str(raw.get("joint_prefix") or "").strip()
|
||||
site.present.add("joint_prefix")
|
||||
return site
|
||||
|
||||
|
||||
def repo_root() -> Path:
|
||||
"""仓库根。**实现只有一份**：``robot_description.repo_root()``。
|
||||
|
||||
这里曾经也是 ``parents[3]`` —— 源码树里对、安装后错（install_sim 是拷贝）。
|
||||
两处各写一份还会各自漂移，所以都改成委托（见 sim_scene.repo_root 的注释）。
|
||||
"""
|
||||
from arm_sim_gazebo.robot_description import repo_root as _robust_root
|
||||
|
||||
return _robust_root()
|
||||
|
||||
|
||||
def resolve_world(
|
||||
cli_world: str = "",
|
||||
root: Optional[Path] = None,
|
||||
fallback: Optional[Path] = None,
|
||||
) -> tuple[Path, str]:
|
||||
"""决定用哪个 world 文件。返回 (路径, 来源说明)。
|
||||
|
||||
优先级：命令行 --world > site.yaml 的 sim.world > 内置默认。
|
||||
|
||||
为什么要有优先级而不是"只认 site.yaml"：site.yaml 是站点级的，而调试时
|
||||
经常要临时换一个世界（比如 in.sdf 里复制一份改着试）。命令行必须能压过它。
|
||||
"""
|
||||
base = Path(root) if root is not None else repo_root()
|
||||
|
||||
if cli_world and cli_world.strip():
|
||||
p = Path(cli_world.strip())
|
||||
# 相对路径按仓库根解析：用户从任意目录启动，写相对路径的直觉都是"相对仓库"。
|
||||
if not p.is_absolute():
|
||||
p = base / p
|
||||
return p, "命令行 --world"
|
||||
|
||||
site = load_sim_site(base)
|
||||
if site.world:
|
||||
p = Path(site.world)
|
||||
if not p.is_absolute():
|
||||
p = base / p
|
||||
return p, f"{SITE_REL} 的 sim.world"
|
||||
|
||||
if fallback is None:
|
||||
raise SimSiteError(
|
||||
"没能确定 world 文件：site.yaml 里 sim.world 为空，也没给内置默认。"
|
||||
)
|
||||
fb = Path(fallback)
|
||||
# 内置默认也要跟前面两条一样解析成绝对路径：调用方（启动器 / launch）
|
||||
# 拿到的必须是绝对路径，否则从别的目录启动时会被解析到错误的位置，
|
||||
# 而那会表现成"找不到 world 文件"，看起来像配置写错了。
|
||||
if not fb.is_absolute():
|
||||
fb = base / fb
|
||||
return fb, "内置默认（site.yaml 未指定 sim.world）"
|
||||
|
||||
|
||||
def unwired_warnings(
|
||||
site: SimSite,
|
||||
actual_joint_prefix: Optional[str] = None,
|
||||
physics_in_world: Optional[bool] = None,
|
||||
) -> List[str]:
|
||||
"""把"site.yaml 里声明了、但当前实现并不消费"的配置报出来。
|
||||
|
||||
这是本模块存在的一半理由：与其让 `physics_enable: false` 静默无效，
|
||||
不如明说"这个键还没接线"。下面两个键目前确实没接：
|
||||
|
||||
physics_enable 世界文件里的重力/步长是写死的，没有按它开关。
|
||||
joint_prefix 仿真关节前缀由 arm.sim.urdf.xacro 的 arm_prefix 决定，
|
||||
且 controllers.yaml 里硬编码了 sim_joint1..7；
|
||||
改 site.yaml 不会传导过去。
|
||||
|
||||
等它们真的接线了，从这里删掉对应分支，告警就自然消失。
|
||||
"""
|
||||
out: List[str] = []
|
||||
|
||||
if "physics_enable" in site.present and physics_in_world is not None:
|
||||
want_on = bool(site.physics_enable)
|
||||
if want_on != bool(physics_in_world):
|
||||
out.append(
|
||||
f"site.yaml 写了 sim.physics_enable={site.physics_enable}，"
|
||||
f"但世界文件当前是物理{'开启' if physics_in_world else '关闭'}状态，"
|
||||
f"而这个键**还没有接线** —— 世界文件里的重力/步长是写死的。\n"
|
||||
f" 想让物理开关生效，得改 world 文件里的 <physics>，"
|
||||
f"或者先把这个键接进启动流程（见 sim_site.py 顶部说明）。"
|
||||
)
|
||||
|
||||
if "joint_prefix" in site.present and actual_joint_prefix is not None:
|
||||
want = site.joint_prefix
|
||||
if want and want != actual_joint_prefix:
|
||||
out.append(
|
||||
f"site.yaml 写了 sim.joint_prefix={want!r}，"
|
||||
f"但实际用的前缀是 {actual_joint_prefix!r}，"
|
||||
f"而这个键**还没有接线**。\n"
|
||||
f" 前缀来自 arm.sim.urdf.xacro 的 arm_prefix，"
|
||||
f"且 config/controllers.yaml 里硬编码了 {actual_joint_prefix}joint1..7 —— "
|
||||
f"三处必须一致，改一处不够。"
|
||||
)
|
||||
|
||||
return out
|
||||
|
||||
|
||||
def physics_enabled_in_world(world_file: Path) -> Optional[bool]:
|
||||
"""世界文件里的物理到底开没开。判据是重力是不是零向量。
|
||||
|
||||
gz 里"关物理"的常见做法就是把 <gravity> 置零（比删掉整个 <physics>
|
||||
更常用，因为 <physics> 还带着步长等其它必需项）。没有 <physics> /
|
||||
<gravity> 时按"开着"算 —— gz 的默认就是开。
|
||||
"""
|
||||
try:
|
||||
text = Path(world_file).read_text(encoding="utf-8", errors="replace")
|
||||
except OSError:
|
||||
return None
|
||||
m = re.search(r"<gravity>\s*([^<]+)\s*</gravity>", text)
|
||||
if not m:
|
||||
return True
|
||||
try:
|
||||
vals = [float(x) for x in m.group(1).split()]
|
||||
except ValueError:
|
||||
return None
|
||||
if len(vals) != 3:
|
||||
return None
|
||||
return any(abs(v) > 1e-12 for v in vals)
|
||||
@@ -0,0 +1,317 @@
|
||||
"""仿真触觉的配置解析、轴向投影与基线跟踪。
|
||||
|
||||
=============================================================================
|
||||
为什么单独一个模块
|
||||
=============================================================================
|
||||
把 Gazebo 的关节力传感器变成 arm_msgs/TactileStatus，真正有内容的是三件事：
|
||||
|
||||
1. 轴向投影 关节坐标系的三向力 → (fnormal, fshearx, fsheary)
|
||||
2. 基线相减 静止时关节要承担手指自重，不减掉就是一个恒定偏置
|
||||
3. 接触判据 接触传感器是**事件式**发布，必须靠超时判"已离开"
|
||||
|
||||
这三件事都是纯数据变换，不需要 ROS 跑起来就能验。所以实现放在这里、用普通
|
||||
函数/类表达，节点（sim_tactile_adapter.py）只做订阅/发布这层壳。
|
||||
好处是它们能被单测钉死 —— 轴向映射搞反、基线算错这类错误，
|
||||
在"力有数值变化"的直观观察下完全看不出异常，只能靠测试兜住。
|
||||
|
||||
轴映射的实测依据与口径见 devices/sim/gazebo/config/tactile.yaml 的注释。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Optional, Sequence, Tuple
|
||||
|
||||
import yaml
|
||||
|
||||
# 默认配置位置（相对仓库根）。与 cameras.yaml 同目录，属"物料而非代码"。
|
||||
DEFAULT_TACTILE_REL = "devices/sim/gazebo/config/tactile.yaml"
|
||||
|
||||
# 轴向只认这三根轴。写成别的一律报错 —— 静默地把 'Y' 当成未知轴处理，
|
||||
# 会让力的语义整体错位而没有任何提示。
|
||||
_VALID_AXES = ("x", "y", "z")
|
||||
|
||||
|
||||
class TactileConfigError(RuntimeError):
|
||||
"""触觉配置有问题时抛出，消息面向使用者。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AxisSpec:
|
||||
"""把关节力传感器的三根轴映射成 Orisys 的三路力。
|
||||
|
||||
``normal`` 是**指尖法向**（压下去的方向）；另两根是切向。
|
||||
符号单独给：实测确认两指压同一个物体读数同号，但保留符号项是为了
|
||||
换夹爪/换传感器时不必改代码。
|
||||
"""
|
||||
|
||||
normal: str = "y"
|
||||
shear_x: str = "z"
|
||||
shear_y: str = "x"
|
||||
sign_normal: float = 1.0
|
||||
sign_shear_x: float = 1.0
|
||||
sign_shear_y: float = 1.0
|
||||
|
||||
def project(self, force: Sequence[float], scale: float = 1.0) -> Tuple[float, float, float]:
|
||||
"""三向力（关节坐标系，牛顿）→ (fnormal, fshearx, fsheary)。
|
||||
|
||||
``scale`` 是 config 里的 force_scale，见该文件头的「单位口径」。
|
||||
"""
|
||||
idx = {a: i for i, a in enumerate(_VALID_AXES)}
|
||||
fx, fy, fz = (float(v) for v in force)
|
||||
raw = {"x": fx, "y": fy, "z": fz}
|
||||
return (
|
||||
self.sign_normal * raw[self.normal] * scale,
|
||||
self.sign_shear_x * raw[self.shear_x] * scale,
|
||||
self.sign_shear_y * raw[self.shear_y] * scale,
|
||||
)
|
||||
|
||||
def shear_indices(self) -> Tuple[int, int]:
|
||||
"""两个切向轴在 (x,y,z) 里的下标，用于把接触点转成 centroid_u/v。"""
|
||||
idx = {a: i for i, a in enumerate(_VALID_AXES)}
|
||||
return idx[self.shear_x], idx[self.shear_y]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TactileSide:
|
||||
"""一"指"的声明。"""
|
||||
|
||||
side: str
|
||||
wrench_topic: str
|
||||
contact_topic: str
|
||||
frame: str
|
||||
|
||||
# --- ROS 侧话题名：与真机 tactile_node 完全一致，应用层不用改 -----------
|
||||
@property
|
||||
def status_topic(self) -> str:
|
||||
return f"/tactile/{self.side}/status"
|
||||
|
||||
@property
|
||||
def image_topic(self) -> str:
|
||||
return f"/tactile/{self.side}/image"
|
||||
|
||||
@property
|
||||
def vector_topic(self) -> str:
|
||||
return f"/tactile/{self.side}/vector"
|
||||
|
||||
# --- Gazebo 侧：桥接的输入 ---------------------------------------------
|
||||
@property
|
||||
def gz_wrench_msg(self) -> str:
|
||||
return "gz.msgs.Wrench"
|
||||
|
||||
@property
|
||||
def gz_contact_msg(self) -> str:
|
||||
return "gz.msgs.Contacts"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TactileSpec:
|
||||
"""整个仿真触觉的声明。"""
|
||||
|
||||
sides: Tuple[TactileSide, ...]
|
||||
axis: AxisSpec
|
||||
publish_hz: float
|
||||
baseline_seconds: float
|
||||
rebaseline_when_idle: bool
|
||||
rebaseline_tau_s: float
|
||||
contact_timeout_s: float
|
||||
min_contact_points: int
|
||||
force_scale: float
|
||||
image_hz: float
|
||||
image_width: int
|
||||
image_height: int
|
||||
unit: str = "N"
|
||||
|
||||
def by_side(self, side: str) -> Optional[TactileSide]:
|
||||
for s in self.sides:
|
||||
if s.side == side:
|
||||
return s
|
||||
return None
|
||||
|
||||
|
||||
def load_tactile(path: Optional[Path] = None, root: Optional[Path] = None) -> TactileSpec:
|
||||
"""读触觉配置。找不到或写错时抛 TactileConfigError，消息里给出修法。"""
|
||||
if path is None:
|
||||
base = root if root is not None else Path.cwd()
|
||||
path = Path(base) / DEFAULT_TACTILE_REL
|
||||
path = Path(path)
|
||||
|
||||
if not path.is_file():
|
||||
raise TactileConfigError(
|
||||
f"触觉配置不存在: {path}\n"
|
||||
f" 它是仿真相机之外另一份「物料配置」：话题名、轴向映射、force_scale、\n"
|
||||
f" 基线口径全在这里（桥接与适配器都从它派生）。缺了它仿真里的触觉\n"
|
||||
f" 完全没有输出。确认仓库根下存在 {DEFAULT_TACTILE_REL}。"
|
||||
)
|
||||
|
||||
try:
|
||||
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except yaml.YAMLError as exc:
|
||||
raise TactileConfigError(f"触觉配置 YAML 解析失败: {path}\n 原因: {exc}") from exc
|
||||
|
||||
raw_sides = doc.get("sides")
|
||||
if not isinstance(raw_sides, list) or not raw_sides:
|
||||
raise TactileConfigError(
|
||||
f"{path} 里没有 sides 列表（或为空）。双指方案要写 left 与 right 两项 ——\n"
|
||||
f" 单指无法复现「旋拧时切向力从哪一侧来」这个判据。"
|
||||
)
|
||||
|
||||
sides: List[TactileSide] = []
|
||||
seen: Dict[str, str] = {}
|
||||
for i, item in enumerate(raw_sides):
|
||||
where = f"{path} 的 sides[{i}]"
|
||||
if not isinstance(item, dict):
|
||||
raise TactileConfigError(f"{where} 不是映射（key: value 形式）。")
|
||||
missing = [
|
||||
k for k in ("side", "wrench_topic", "contact_topic", "frame")
|
||||
if not str(item.get(k, "")).strip()
|
||||
]
|
||||
if missing:
|
||||
raise TactileConfigError(
|
||||
f"{where} 缺字段: {', '.join(missing)}。\n"
|
||||
f" wrench_topic/contact_topic 必须与 arm.sim.sensors.xacro 里写的\n"
|
||||
f" <topic> 完全一致 —— 对不上时适配器只是订阅了一个不存在的话题，\n"
|
||||
f" 不报错、status 永远是零。"
|
||||
)
|
||||
side = str(item["side"]).strip()
|
||||
if side in seen:
|
||||
raise TactileConfigError(f"{where} 的 side 重复: {side}")
|
||||
seen[side] = where
|
||||
sides.append(
|
||||
TactileSide(
|
||||
side=side,
|
||||
wrench_topic=str(item["wrench_topic"]).strip(),
|
||||
contact_topic=str(item["contact_topic"]).strip(),
|
||||
frame=str(item["frame"]).strip(),
|
||||
)
|
||||
)
|
||||
|
||||
raw_axis = doc.get("axis") or {}
|
||||
raw_sign = doc.get("sign") or {}
|
||||
if not isinstance(raw_axis, dict) or not isinstance(raw_sign, dict):
|
||||
raise TactileConfigError(f"{path} 的 axis / sign 必须是映射。")
|
||||
|
||||
axis = AxisSpec(
|
||||
normal=_axis(raw_axis.get("normal", "y"), path, "axis.normal"),
|
||||
shear_x=_axis(raw_axis.get("shear_x", "z"), path, "axis.shear_x"),
|
||||
shear_y=_axis(raw_axis.get("shear_y", "x"), path, "axis.shear_y"),
|
||||
sign_normal=_num(raw_sign.get("normal", 1.0), path, "sign.normal"),
|
||||
sign_shear_x=_num(raw_sign.get("shear_x", 1.0), path, "sign.shear_x"),
|
||||
sign_shear_y=_num(raw_sign.get("shear_y", 1.0), path, "sign.shear_y"),
|
||||
)
|
||||
if len({axis.normal, axis.shear_x, axis.shear_y}) != 3:
|
||||
raise TactileConfigError(
|
||||
f"{path} 的 axis 里三根轴必须互不相同（当前 normal={axis.normal}, "
|
||||
f"shear_x={axis.shear_x}, shear_y={axis.shear_y}）。\n"
|
||||
f" 重复意味着有两个分量取自同一根轴，另有一根轴被丢掉 —— "
|
||||
f"力的语义会静默错位。"
|
||||
)
|
||||
|
||||
spec = TactileSpec(
|
||||
sides=tuple(sides),
|
||||
axis=axis,
|
||||
publish_hz=_num(doc.get("publish_hz", 20.0), path, "publish_hz", positive=True),
|
||||
baseline_seconds=_num(doc.get("baseline_seconds", 2.0), path, "baseline_seconds"),
|
||||
rebaseline_when_idle=bool(doc.get("rebaseline_when_idle", True)),
|
||||
rebaseline_tau_s=_num(doc.get("rebaseline_tau_s", 3.0), path, "rebaseline_tau_s"),
|
||||
contact_timeout_s=_num(
|
||||
doc.get("contact_timeout_s", 0.25), path, "contact_timeout_s", positive=True
|
||||
),
|
||||
min_contact_points=int(doc.get("min_contact_points", 1)),
|
||||
force_scale=_num(doc.get("force_scale", 1.0), path, "force_scale"),
|
||||
image_hz=_num(doc.get("image_hz", 0.0), path, "image_hz"),
|
||||
image_width=int(doc.get("image_width", 64)),
|
||||
image_height=int(doc.get("image_height", 64)),
|
||||
unit=str(doc.get("unit", "N")),
|
||||
)
|
||||
return spec
|
||||
|
||||
|
||||
def _axis(value: object, path: Path, key: str) -> str:
|
||||
axis = str(value).strip().lower()
|
||||
if axis not in _VALID_AXES:
|
||||
raise TactileConfigError(
|
||||
f"{path} 的 {key} 必须是 x/y/z 之一，当前是 {value!r}。"
|
||||
)
|
||||
return axis
|
||||
|
||||
|
||||
def _num(value: object, path: Path, key: str, positive: bool = False) -> float:
|
||||
try:
|
||||
out = float(value)
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise TactileConfigError(f"{path} 的 {key} 不是数字: {value!r}") from exc
|
||||
if positive and out <= 0.0:
|
||||
raise TactileConfigError(f"{path} 的 {key} 必须为正，当前是 {out}")
|
||||
return out
|
||||
|
||||
|
||||
def bridge_args(spec: TactileSpec) -> List[str]:
|
||||
"""生成 ros_gz_bridge parameter_bridge 的话题规则。
|
||||
|
||||
只做**传输**：gz 消息类型 → ROS 消息类型，话题名保持 gz 侧原样。
|
||||
改名与语义（轴向、基线、单位）全在适配器里做 —— 桥接器改不了 frame_id，
|
||||
也做不了基线相减，越权只会把"在哪一层改"这件事弄乱。
|
||||
"""
|
||||
args: List[str] = []
|
||||
for s in spec.sides:
|
||||
args.append(
|
||||
f"{s.wrench_topic}@geometry_msgs/msg/Wrench[{s.gz_wrench_msg}"
|
||||
)
|
||||
args.append(
|
||||
f"{s.contact_topic}@ros_gz_interfaces/msg/Contacts[{s.gz_contact_msg}"
|
||||
)
|
||||
return args
|
||||
|
||||
|
||||
class BaselineTracker:
|
||||
"""按"相对静止基线取增量"的口径跟踪零力读数。
|
||||
|
||||
两段式：
|
||||
预热期（前 baseline_seconds 秒）：等权均值。用均值而不是 EMA 是因为
|
||||
一开始没有任何先验，EMA 从 0 爬升需要很久。
|
||||
稳态期：仅在**确认无接触**时用时间常数 tau 做慢跟踪。
|
||||
为什么需要慢跟踪而不是"一次性基线"：force_torque 读的是关节传递力，
|
||||
静止时它等于远端那段手指的自重，而**这个自重项随机械臂姿态变化** ——
|
||||
一次性基线会在换姿态后留下残余。只在无接触时更新，是为了不把真实
|
||||
接触当成新基线吃掉（那会让接触力永远回到零，症状是"夹住了但力为 0"）。
|
||||
"""
|
||||
|
||||
def __init__(self, baseline_seconds: float, tau_s: float, enabled: bool = True) -> None:
|
||||
self._warmup = max(0.0, float(baseline_seconds))
|
||||
self._tau = max(0.0, float(tau_s))
|
||||
self._enabled = bool(enabled)
|
||||
self._sum: Dict[str, float] = {}
|
||||
self._count: Dict[str, int] = {}
|
||||
self._value: Dict[str, float] = {}
|
||||
self._elapsed = 0.0
|
||||
|
||||
@property
|
||||
def warmed_up(self) -> bool:
|
||||
return self._elapsed >= self._warmup
|
||||
|
||||
def add(self, key: str, raw: float, dt: float, in_contact: bool) -> None:
|
||||
"""喂一个原始读数。``dt`` 是距上次的秒数。"""
|
||||
if in_contact:
|
||||
# 接触期间既不预热也不跟踪：接触力绝不能进基线。
|
||||
return
|
||||
if not self.warmed_up:
|
||||
self._sum[key] = self._sum.get(key, 0.0) + float(raw)
|
||||
self._count[key] = self._count.get(key, 0) + 1
|
||||
self._value[key] = self._sum[key] / max(1, self._count[key])
|
||||
return
|
||||
if not self._enabled or self._tau <= 0.0 or key not in self._value:
|
||||
return
|
||||
alpha = min(1.0, max(0.0, dt) / self._tau)
|
||||
self._value[key] += (float(raw) - self._value[key]) * alpha
|
||||
|
||||
def advance(self, dt: float) -> None:
|
||||
"""推进预热计时。必须在每轮循环调用一次（即使这一轮没有读数）。"""
|
||||
self._elapsed += max(0.0, float(dt))
|
||||
|
||||
def value(self, key: str) -> float:
|
||||
return self._value.get(key, 0.0)
|
||||
|
||||
def delta(self, key: str, raw: float) -> float:
|
||||
"""原始读数 → 相对基线的增量。"""
|
||||
return float(raw) - self.value(key)
|
||||
+389
@@ -0,0 +1,389 @@
|
||||
"""仿真触觉适配器：Gazebo 的关节力/接触传感器 → arm_msgs/TactileStatus。
|
||||
|
||||
=============================================================================
|
||||
它在链路上的位置
|
||||
=============================================================================
|
||||
gz /tactile/<side>/wrench (gz.msgs.Wrench, 持续 ~100Hz)
|
||||
gz /tactile/<side>/raw (gz.msgs.Contacts, 只在接触时)
|
||||
│
|
||||
├─ ros_gz_bridge parameter_bridge（只搬类型，不改语义）
|
||||
│
|
||||
└─ 本节点
|
||||
├ 轴向投影 三向力 → fnormal/fshearx/fsheary（映射实测，见 config）
|
||||
├ 基线相减 静止时关节要承担手指自重，不减就是一个恒定偏置
|
||||
├ 接触判据 超时判"已离开"（接触传感器是事件式发布）
|
||||
└ 变送 /tactile/<side>/status （arm_msgs/TactileStatus）
|
||||
|
||||
话题名、消息类型、字段口径全部与真机 tactile_node 对齐，所以下游
|
||||
（compliant_descent、stage3_search_node）**一行都不用改**就能吃仿真的数据，
|
||||
这正是"同一套接口、换后端"的契约。
|
||||
|
||||
=============================================================================
|
||||
三个必须知道的坑（都会静默）
|
||||
=============================================================================
|
||||
1) 接触传感器的消息是**事件式**的：不接触时一条都不发。
|
||||
若适配器只在收到消息时才发布 status，那么"离开工件"期间下游会认为
|
||||
话题断流（stage3_search_node 的 _tac_fresh 会直接 SearchAbort）。
|
||||
所以本节点**按 publish_hz 恒定发布**，接触状态由"最近是否收到过"决定。
|
||||
真机 Orisys 也是恒定发布的，这里必须与真机一致。
|
||||
|
||||
2) 力的话题来自 force_torque 传感器，而它依赖 world 里的
|
||||
forcetorque-system 插件被显式声明（只要 world 声明了任何 <plugin>，
|
||||
gz 就不再加载默认系统列表）。插件漏了的话**话题根本不出现**、
|
||||
日志一片空白。所以本节点在启动几秒后若一个 wrench 都没收到，
|
||||
会明确报出来并指出该查哪里。
|
||||
|
||||
3) 单位不是牛顿等价值就可能是错的：Gazebo 给的是牛顿，真机给的是形变
|
||||
任意单位。换算靠 config 里的 force_scale，见 tactile.yaml 文件头的
|
||||
「单位口径」。选错不报错，只表现为阈值永远不触发或一碰就炸。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import time
|
||||
from pathlib import Path
|
||||
from typing import Dict, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.time import Time
|
||||
|
||||
from arm_msgs.msg import TactileStatus
|
||||
from geometry_msgs.msg import Wrench
|
||||
from sensor_msgs.msg import Image
|
||||
from tf2_ros import Buffer, TransformListener
|
||||
|
||||
# ros_gz_interfaces 只在仿真里用得到，而且是 conda 装的（不在本工作区）。
|
||||
# 万一它的 typesupport 加载不了（见下），我们只丢掉"接触点"这一路信息 ——
|
||||
# 力本身走 geometry_msgs/Wrench，不受影响。所以这里**容错导入**：
|
||||
# 宁可用退化模式跑，也不要整个适配器起不来。
|
||||
_contacts_err: Optional[str] = None
|
||||
try:
|
||||
from ros_gz_interfaces.msg import Contacts
|
||||
except Exception as _exc: # pragma: no cover - 取决于运行环境
|
||||
Contacts = None # type: ignore[assignment]
|
||||
_contacts_err = f"{type(_exc).__name__}: {_exc}"
|
||||
|
||||
from .sim_tactile import (
|
||||
DEFAULT_TACTILE_REL,
|
||||
BaselineTracker,
|
||||
TactileConfigError,
|
||||
TactileSide,
|
||||
TactileSpec,
|
||||
load_tactile,
|
||||
)
|
||||
|
||||
# 启动后多久检查"到底有没有收到力数据"。要给桥接器与 gz 一点时间。
|
||||
TOPIC_CHECK_DELAY_S = 8.0
|
||||
|
||||
|
||||
def _depth_placeholder(width: int, height: int) -> np.ndarray:
|
||||
"""占位深度图：32FC1、全 NaN。
|
||||
|
||||
为什么是 NaN 而不是全 0：全 0 在深度语义里是"距离 0 米"，会被下游当成
|
||||
一个贴脸的平面；NaN 是"无读数"，不会伪装成有效数据。
|
||||
"""
|
||||
return np.full((height, width), np.nan, dtype=np.float32)
|
||||
|
||||
|
||||
def _vector_placeholder(width: int, height: int) -> np.ndarray:
|
||||
"""占位力矢量图：32FC2（每像素 dx, dy），全 NaN。"""
|
||||
return np.full((height, width, 2), np.nan, dtype=np.float32)
|
||||
|
||||
|
||||
class SimTactileAdapter(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim_tactile_adapter")
|
||||
|
||||
self.declare_parameter("tactile_file", "")
|
||||
# ⚠️ 不要 declare_parameter("use_sim_time", ...)：launch 会把它作为
|
||||
# 参数文件传进来，rclpy 那时**已经自动声明过**，再声明一次会直接抛
|
||||
# ParameterAlreadyDeclaredException 让节点起不来（实测踩过）。
|
||||
# 相机适配器同样只声明自己的参数。
|
||||
|
||||
raw_path = str(self.get_parameter("tactile_file").value or "")
|
||||
path = Path(raw_path) if raw_path else Path.cwd() / DEFAULT_TACTILE_REL
|
||||
try:
|
||||
self.spec: TactileSpec = load_tactile(path)
|
||||
except TactileConfigError as exc:
|
||||
# 配置错误必须让节点直接失败：静默启动一个"永远零力"的适配器
|
||||
# 比不启动更糟 —— 它看起来在工作。
|
||||
self.get_logger().error(f"触觉配置有问题：\n{exc}")
|
||||
raise
|
||||
|
||||
self.get_logger().info(
|
||||
f"触觉：单位={self.spec.unit} force_scale={self.spec.force_scale} "
|
||||
f"发布率={self.spec.publish_hz}Hz 基线={self.spec.baseline_seconds}s"
|
||||
f" 轴向 normal={self.spec.axis.normal} "
|
||||
f"shear_x={self.spec.axis.shear_x} shear_y={self.spec.axis.shear_y}"
|
||||
)
|
||||
|
||||
self._tf_buffer = Buffer()
|
||||
self._tf_listener = TransformListener(self._tf_buffer, self)
|
||||
|
||||
# 每指的状态。键都是 side。
|
||||
self._wrench: Dict[str, Tuple[float, float, float]] = {}
|
||||
self._wrench_stamp: Dict[str, float] = {}
|
||||
self._contact_point: Dict[str, Optional[Tuple[float, float, float]]] = {}
|
||||
self._contact_stamp: Dict[str, float] = {}
|
||||
self._got_wrench: Dict[str, bool] = {}
|
||||
self._got_contact: Dict[str, bool] = {}
|
||||
|
||||
self._baseline = BaselineTracker(
|
||||
self.spec.baseline_seconds,
|
||||
self.spec.rebaseline_tau_s,
|
||||
enabled=self.spec.rebaseline_when_idle,
|
||||
)
|
||||
self._last_tick = time.monotonic()
|
||||
|
||||
self._pub_status = {}
|
||||
self._pub_image: Dict[str, Tuple[object, object, float, float]] = {}
|
||||
self._contact_unavailable = False
|
||||
for s in self.spec.sides:
|
||||
self._got_wrench[s.side] = False
|
||||
self._got_contact[s.side] = False
|
||||
self._pub_status[s.side] = self.create_publisher(
|
||||
TactileStatus, s.status_topic, 10
|
||||
)
|
||||
self.create_subscription(
|
||||
Wrench,
|
||||
s.wrench_topic,
|
||||
self._make_wrench_cb(s.side),
|
||||
10,
|
||||
)
|
||||
if Contacts is not None:
|
||||
self.create_subscription(
|
||||
Contacts,
|
||||
s.contact_topic,
|
||||
self._make_contact_cb(s.side),
|
||||
10,
|
||||
)
|
||||
else:
|
||||
self._contact_unavailable = True
|
||||
if self.spec.image_hz > 0.0:
|
||||
self._pub_image[s.side] = (
|
||||
self.create_publisher(Image, s.image_topic, 3),
|
||||
self.create_publisher(Image, s.vector_topic, 3),
|
||||
self.spec.image_hz,
|
||||
0.0,
|
||||
)
|
||||
self.get_logger().warn(
|
||||
f"仿真里没有弹性体成像：{s.image_topic} 与 {s.vector_topic} "
|
||||
f"发布的是**占位图**（32FC1 / 32FC2 全 NaN），话题与消息结构"
|
||||
f"与真机一致，但内容没有物理含义。需要真实图像时请关掉 "
|
||||
f"image_hz（设为 0）。"
|
||||
)
|
||||
|
||||
period = 1.0 / max(1e-3, self.spec.publish_hz)
|
||||
self.create_timer(period, self._tick)
|
||||
self._check_at = time.monotonic() + TOPIC_CHECK_DELAY_S
|
||||
self._checked = False
|
||||
|
||||
if self._contact_unavailable:
|
||||
detail = f"（导入失败：{_contacts_err}）" if _contacts_err else ""
|
||||
self.get_logger().warn(
|
||||
f"ros_gz_interfaces/msg/Contacts 用不了{detail}，"
|
||||
f"本节点进入退化模式：\n"
|
||||
f" 仍会发 TactileStatus（fnormal/fshearx/fsheary、connected、"
|
||||
f"接触去基线都正常），\n"
|
||||
f" 但 contact 恒为 False、centroid_u/v 恒为 0"
|
||||
f"（这两项依赖接触点消息）。\n"
|
||||
f" 三力判据（本项目 S07 用的那个）不受影响。\n"
|
||||
f" 要修：在**干净 shell** 里 source scripts/sim_env.sh（它会清掉真机"
|
||||
f"环境残留的 PYTHONPATH/LD_LIBRARY_PATH 并补上 $CONDA_PREFIX/lib）。"
|
||||
)
|
||||
|
||||
# --- 订阅回调 ---------------------------------------------------------
|
||||
def _make_wrench_cb(self, side: str):
|
||||
def cb(msg: Wrench) -> None:
|
||||
self._got_wrench[side] = True
|
||||
self._wrench[side] = (
|
||||
float(msg.force.x),
|
||||
float(msg.force.y),
|
||||
float(msg.force.z),
|
||||
)
|
||||
self._wrench_stamp[side] = time.monotonic()
|
||||
|
||||
return cb
|
||||
|
||||
def _make_contact_cb(self, side: str):
|
||||
def cb(msg: Contacts) -> None:
|
||||
pts = []
|
||||
for c in msg.contacts:
|
||||
for p in c.positions:
|
||||
pts.append((float(p.x), float(p.y), float(p.z)))
|
||||
if len(pts) < max(1, self.spec.min_contact_points):
|
||||
return
|
||||
n = float(len(pts))
|
||||
self._got_contact[side] = True
|
||||
self._contact_point[side] = (
|
||||
sum(p[0] for p in pts) / n,
|
||||
sum(p[1] for p in pts) / n,
|
||||
sum(p[2] for p in pts) / n,
|
||||
)
|
||||
self._contact_stamp[side] = time.monotonic()
|
||||
|
||||
return cb
|
||||
|
||||
# --- 主循环 -----------------------------------------------------------
|
||||
def _tick(self) -> None:
|
||||
now = time.monotonic()
|
||||
dt = now - self._last_tick
|
||||
self._last_tick = now
|
||||
self._baseline.advance(dt)
|
||||
|
||||
for s in self.spec.sides:
|
||||
raw = self._wrench.get(s.side)
|
||||
in_contact = self._in_contact(s, now)
|
||||
|
||||
if raw is None:
|
||||
# 还没收到过力数据。仍然发布（下游靠"有没有帧"判活），
|
||||
# 但发的是零 + connected=False，让下游能区分"没数据"与"零力"。
|
||||
self._publish(s.side, connected=False, contact=False,
|
||||
forces=(0.0, 0.0, 0.0), stamp=now)
|
||||
continue
|
||||
|
||||
for i, axis in enumerate(("x", "y", "z")):
|
||||
self._baseline.add(f"{s.side}_{axis}", raw[i], dt, in_contact)
|
||||
|
||||
delta = tuple(
|
||||
self._baseline.delta(f"{s.side}_{axis}", raw[i]) for i, axis in enumerate(("x", "y", "z"))
|
||||
)
|
||||
fn, fsx, fsy = self.spec.axis.project(delta, self.spec.force_scale)
|
||||
# 真机 fnormal 也是非负的（法向只能压不能拉），这里保持一致。
|
||||
fn = max(0.0, fn)
|
||||
self._publish(s.side, connected=True, contact=in_contact,
|
||||
forces=(fn, fsx, fsy), stamp=now, side_spec=s)
|
||||
|
||||
self._maybe_check_topics(now)
|
||||
|
||||
def _in_contact(self, s: TactileSide, now: float) -> bool:
|
||||
"""接触 = 最近 contact_timeout_s 内收到过接触消息。
|
||||
|
||||
⚠️ 不能写成"收到消息就一直算接触"：接触传感器是事件式的，
|
||||
离开工件后不会发"取消接触"的消息，那样 contact 会永远停在 True。
|
||||
"""
|
||||
stamp = self._contact_stamp.get(s.side)
|
||||
return stamp is not None and (now - stamp) <= self.spec.contact_timeout_s
|
||||
|
||||
def _centroid(self, s: TactileSide) -> Tuple[float, float]:
|
||||
"""接触点在指尖坐标系里沿两个切向轴的偏移（毫米）→ (u, v)。
|
||||
|
||||
真机的 centroid_u/v 是形变图的像素坐标；仿真没有成像，用"接触点在指腹
|
||||
平面上的位置"表达同一件事 —— 同样是"接触发生在指腹的哪里"。
|
||||
下游目前不消费这两个字段，所以量纲不同不会造成影响；
|
||||
真要跨仿真/真机共用，这里是最需要先统一的一处。
|
||||
"""
|
||||
pt = self._contact_point.get(s.side)
|
||||
if pt is None:
|
||||
return (0.0, 0.0)
|
||||
try:
|
||||
tr = self._tf_buffer.lookup_transform(s.frame, "world", Time())
|
||||
except Exception:
|
||||
return (0.0, 0.0)
|
||||
t = tr.transform.translation
|
||||
q = tr.transform.rotation
|
||||
# world → frame：位置平移 + 用共轭四元数旋转
|
||||
px, py, pz = pt[0] - t.x, pt[1] - t.y, pt[2] - t.z
|
||||
# 逆旋转：q* v q
|
||||
qx, qy, qz, qw = -q.x, -q.y, -q.z, q.w
|
||||
ix = qw * px + qy * pz - qz * py
|
||||
iy = qw * py + qz * px - qx * pz
|
||||
iz = qw * pz + qx * py - qy * px
|
||||
iw = -qx * px - qy * py - qz * pz
|
||||
vx = ix * qw + iw * -qx + iy * -qz - iz * -qy
|
||||
vy = iy * qw + iw * -qy + iz * -qx - ix * -qz
|
||||
vz = iz * qw + iw * -qz + ix * -qy - iy * -qx
|
||||
i_sx, i_sy = self.spec.axis.shear_indices()
|
||||
comp = (vx, vy, vz)
|
||||
return (comp[i_sx] * 1000.0, comp[i_sy] * 1000.0)
|
||||
|
||||
def _publish(self, side: str, connected: bool, contact: bool,
|
||||
forces: Tuple[float, float, float], stamp: float,
|
||||
side_spec: Optional[TactileSide] = None) -> None:
|
||||
msg = TactileStatus()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = f"{side}_tactile"
|
||||
msg.side = side
|
||||
msg.connected = connected
|
||||
msg.contact = contact
|
||||
msg.fnormal = float(forces[0])
|
||||
msg.fshearx = float(forces[1])
|
||||
msg.fsheary = float(forces[2])
|
||||
# 仿真没有光学帧率。填 0 而不是编一个 30 —— 假数据比缺数据更难查。
|
||||
msg.fps = 0.0
|
||||
if contact and side_spec is not None:
|
||||
msg.centroid_u, msg.centroid_v = (float(v) for v in self._centroid(side_spec))
|
||||
else:
|
||||
msg.centroid_u = 0.0
|
||||
msg.centroid_v = 0.0
|
||||
msg.error_msg = ""
|
||||
self._pub_status[side].publish(msg)
|
||||
|
||||
pub = self._pub_image.get(side)
|
||||
if pub is not None:
|
||||
p_img, p_vec, hz, last = pub
|
||||
now = time.monotonic()
|
||||
if now - last >= 1.0 / max(1e-3, hz):
|
||||
self._pub_image[side] = (p_img, p_vec, hz, now)
|
||||
self._publish_placeholders(side, p_img, p_vec)
|
||||
|
||||
def _publish_placeholders(self, side: str, p_img, p_vec) -> None:
|
||||
stamp = self.get_clock().now().to_msg()
|
||||
w, h = self.spec.image_width, self.spec.image_height
|
||||
|
||||
depth = _depth_placeholder(w, h)
|
||||
img = Image()
|
||||
img.header.stamp = stamp
|
||||
img.header.frame_id = f"{side}_tactile"
|
||||
img.height, img.width = h, w
|
||||
img.encoding = "32FC1"
|
||||
img.is_bigendian = False
|
||||
img.step = w * 4
|
||||
img.data = depth.tobytes()
|
||||
p_img.publish(img)
|
||||
|
||||
vec = _vector_placeholder(w, h)
|
||||
vimg = Image()
|
||||
vimg.header.stamp = stamp
|
||||
vimg.header.frame_id = f"{side}_tactile"
|
||||
vimg.height, vimg.width = h, w
|
||||
vimg.encoding = "32FC2"
|
||||
vimg.is_bigendian = False
|
||||
vimg.step = w * 8
|
||||
vimg.data = vec.tobytes()
|
||||
p_vec.publish(vimg)
|
||||
|
||||
def _maybe_check_topics(self, now: float) -> None:
|
||||
"""启动几秒后确认力话题真的有数据。
|
||||
|
||||
这一步存在的唯一理由：漏声明 forcetorque-system 插件时，gz 不报错、
|
||||
话题不出现、本节点也照常发零力 —— 整条链路"看起来在工作"。
|
||||
主动检查并指出可能原因是唯一能打破这种静默的地方。
|
||||
"""
|
||||
if self._checked or now < self._check_at:
|
||||
return
|
||||
self._checked = True
|
||||
missing = [s.side for s in self.spec.sides if not self._got_wrench[s.side]]
|
||||
if not missing:
|
||||
return
|
||||
self.get_logger().error(
|
||||
f"启动 {TOPIC_CHECK_DELAY_S:.0f}s 后仍未收到力数据（{'/'.join(missing)}）。\n"
|
||||
f" 最可能的原因：world 文件里没有声明 forcetorque-system 插件。\n"
|
||||
f" 只要 world 声明了任何 <plugin>，gz 就不再加载默认系统列表，于是\n"
|
||||
f" force_torque 传感器**静默失效**：话题不出现、日志空白。\n"
|
||||
f" 检查 devices/sim/gazebo/worlds/assembly.world 里是否有\n"
|
||||
f" libignition-gazebo6-forcetorque-system.so 那一项。"
|
||||
)
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = SimTactileAdapter()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
+495
@@ -0,0 +1,495 @@
|
||||
#!/usr/bin/env python3
|
||||
"""仿真"真值感知"：把 Gazebo 里**已知的真实位姿**当成视觉测量发给伺服。
|
||||
|
||||
为什么要这个节点
|
||||
----------------
|
||||
真机的视觉链是 ``perception_node``（YOLO/ONNX）→ 话题
|
||||
``visual_servo/filtered_point``。仿真环境（ros_gz_sim conda）里没有
|
||||
torch/ultralytics，而且这台机器**离线**装不上，于是 perception_node 一启动就死。
|
||||
|
||||
死法还很误导人：S00 只看"权重文件在不在"（在），S04 一阶段则一直等
|
||||
``/visual_servo/locked``，等满 150s 报"没锁定（视野里没工件？）"——
|
||||
现场看到的是"算法不收敛/标定不准"，真因是"仿真里根本没有推理后端"。
|
||||
|
||||
仿真里其实**不需要推理**：工件摆在哪、长什么样都是已知的
|
||||
（``scene.yaml`` 的绝对位姿 + 模型自己的几何）。伺服要的那个"相机系下的
|
||||
目标点"可以直接算出来：
|
||||
|
||||
世界系目标点 = 物体世界 XY 轮廓中心 + 高度（模型顶面） ＋ 可选局部偏移
|
||||
相机系目标点 = T_cam_world · 世界系目标点 （T 由 TF 查，不自己算）
|
||||
|
||||
发布 → ``PointStamped(visual_servo/filtered_point)``
|
||||
与 perception_node **同话题、同类型**，所以下游（servo_node、
|
||||
整条装配流程）一个字都不用改。
|
||||
|
||||
⚠️ 它验证的是**控制那一半**（坐标变换 / 手眼 / 伺服收敛 / 流程编排），
|
||||
不是感知那一半。真值箱体没有像素噪声、没有遮挡、没有光照变化，也没有
|
||||
"算法把孔认成别的"这类错。所以：
|
||||
|
||||
能跑通 = "控制侧与接口是对的"
|
||||
不能跑通 ≠ "真机上也能跑通" ← 规律写在
|
||||
devices/sim/gazebo/README.md 的「仿真里哪些模块用真值替代」一节
|
||||
|
||||
绝不做的事
|
||||
----------
|
||||
* 目标在相机背后 / 相机查不到 TF 时**不发假点**（和 perception_node 的
|
||||
"没有可用深度就不发点"同一条规矩）。宁可让伺服看门狗超时，也不给它一个
|
||||
幻影目标 —— 后者会让机械臂朝错误方向走，而且看起来像"标定坏了"。
|
||||
* **物体被撞走了就不再发点**。真值感知的整个前提是"物体就在 scene.yaml 写的位置"，
|
||||
前提破了，算出来的就是**假测量**。所以本节点自己订阅 ``/world/<world>/pose/info``
|
||||
（scene.yaml 的 ``pose_info`` 段）盯着部件的实际位姿，偏离名义位姿超过容差就停下
|
||||
并**指名报错**。
|
||||
为什么必须由这一侧盯（2026-09-24 的教训）：``perception.source=truth`` 是纯几何的，
|
||||
它从不问 Gazebo 物体现在在哪。实测工件掉进台面通槽、落到地面之后，"真值"开始撒谎，
|
||||
而现象全都指向别处（二阶段报"两孔间距 169mm 不像 k1/k2"、S07 一路下压不接触），
|
||||
看起来像检测/标定/阈值坏了 —— 真正的原因只有一个，却没有任何一处日志说出来。
|
||||
**谁假设的，谁负责查。**
|
||||
* 不自己算四元数→旋转矩阵（复用 ``handeye_lib.quat_xyzw_to_R``）。
|
||||
这段换算以前在 4 个 launch 文件里各抄了一遍、错了 3 处，症状是
|
||||
"总能到附近、永远差一点"。数学实现只能有一份。
|
||||
* 不自己算四元数→旋转矩阵（复用 ``handeye_lib.quat_xyzw_to_R``）。
|
||||
这段换算以前在 4 个 launch 文件里各抄了一遍、错了 3 处，症状是
|
||||
"总能到附近、永远差一点"。数学实现只能有一份。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Dict, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PointStamped
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
from rclpy.time import Time
|
||||
from tf2_ros import Buffer, TransformListener
|
||||
from tf2_msgs.msg import TFMessage
|
||||
|
||||
from arm_sim_gazebo import sim_scene
|
||||
|
||||
#: 与 perception_node / servo_node 一致：点话题用 BEST_EFFORT + KEEP_LAST(5)。
|
||||
#: 匹配不上 QoS 的后果是"话题在、消息发了、订阅方一条也收不到"，
|
||||
#: 而两边单独看都正常 —— 这类问题排查成本极高，所以直接抄同一份配置。
|
||||
POINT_QOS = QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
)
|
||||
|
||||
#: 目标点离相机光心太近（或跑到背后）就不发：z<=0 在相机系里是"背后"。
|
||||
MIN_CAM_Z_M = 0.01
|
||||
|
||||
|
||||
class TruthError(RuntimeError):
|
||||
"""真值目标解不出来（场景 / 模型 / 参数有问题）。
|
||||
|
||||
单独一个异常类型是为了让"配置错了"和"TF 还没到"分开处理：
|
||||
前者要直接起不来（早报），后者只该刷限流告警（等机器人动起来就有了）。
|
||||
"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TruthTarget:
|
||||
"""真值目标：一个**世界系**点，以及它是怎么算出来的（给人看）。"""
|
||||
|
||||
object_name: str
|
||||
world_xyz: Tuple[float, float, float]
|
||||
note: str
|
||||
|
||||
|
||||
def target_point_world(
|
||||
spec: sim_scene.SceneSpec,
|
||||
obj: sim_scene.SceneObject,
|
||||
offset_m: Sequence[float] = (0.0, 0.0, 0.0),
|
||||
root: Optional[Path] = None,
|
||||
) -> TruthTarget:
|
||||
"""物体顶面中心（世界系）＋ 物体局部系偏移。
|
||||
|
||||
为什么取"顶面中心"：真机 perception_node 默认 ``feature_mode: plane``，
|
||||
发的是检测框在**工件上表面**上的几何中心。真值要想替得住它，就得发同一个
|
||||
物理点；差一个板厚（12mm）会让对比结论完全反过来。
|
||||
|
||||
为什么用 ``sim_scene.footprint`` 而不是自己按 yaw 算：
|
||||
轮廓换算里已经含了 yaw（``world_boxes`` 里有且只有一份实现），
|
||||
自己再写一遍迟早不一致，而"哪个才是对的"很难判断。
|
||||
|
||||
``offset_m`` 是**物体局部系**里的偏移（会跟着 yaw 转），用途例如：
|
||||
把目标从"板中心"挪到 k1 孔 ``[0, -0.029, 0]``，用来单独验证二阶段对孔。
|
||||
"""
|
||||
fp = sim_scene.footprint(spec, obj, root)
|
||||
height = sim_scene.model_height(spec, obj, root)
|
||||
if fp is None or height is None:
|
||||
raise TruthError(
|
||||
f"读不出 {obj.name!r} 的几何（模型 {obj.model}）。\n"
|
||||
f" 期望：{sim_scene.model_sdf_path(spec, obj, root)} 里有 <collision>\n"
|
||||
f" 修法：确认 models_dir 指对了、模型目录里有 model.sdf 且至少一个碰撞体。"
|
||||
)
|
||||
vals = [float(v) for v in offset_m]
|
||||
if len(vals) != 3:
|
||||
# 长度不对必须报错，不能"缺的当 0"：把 [0.03] 当成 (0.03,0,0) 还是
|
||||
# (0,0.03,0) 全凭猜，而猜错的表现是"偏了一点"，很难归因。
|
||||
raise TruthError(
|
||||
f"offset_m 要给 3 个数（物体局部系 xyz，单位米），现在给的是 {list(offset_m)!r}。"
|
||||
)
|
||||
ox, oy, oz = vals
|
||||
cx = 0.5 * (fp[0] + fp[1])
|
||||
cy = 0.5 * (fp[2] + fp[3])
|
||||
c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw))
|
||||
x = cx + c * ox - s * oy
|
||||
y = cy + s * ox + c * oy
|
||||
z = float(obj.z) + float(height) + oz
|
||||
note = (
|
||||
f"{obj.name}（模型 {obj.model}）"
|
||||
f"轮廓中心 ({cx:.3f},{cy:.3f}) + 高 {height * 1000:.1f}mm"
|
||||
f" + 局部偏移 ({ox * 1000:+.0f},{oy * 1000:+.0f},{oz * 1000:+.0f})mm"
|
||||
f" yaw={math.degrees(float(obj.yaw)):.1f}°"
|
||||
)
|
||||
return TruthTarget(object_name=obj.name, world_xyz=(x, y, z), note=note)
|
||||
|
||||
|
||||
def pick_object(spec: sim_scene.SceneSpec, name: str) -> sim_scene.SceneObject:
|
||||
"""按名字找物体；找不到时把**现有名字**列出来（改配置时不用再翻文件）。"""
|
||||
want = (name or "").strip() or spec.operating.part_object
|
||||
for obj in spec.objects:
|
||||
if obj.name == want:
|
||||
return obj
|
||||
raise TruthError(
|
||||
f"场景 {spec.source} 里没有名为 {want!r} 的物体。\n"
|
||||
f" 现有物体：{', '.join(o.name for o in spec.objects) or '（空）'}\n"
|
||||
f" 修法：改 site.yaml 的 perception.truth.object，"
|
||||
f"或确认 scene.yaml 的 objects 里确实有它。"
|
||||
)
|
||||
|
||||
|
||||
def load_truth_target(
|
||||
scene_file: Optional[Path],
|
||||
object_name: str,
|
||||
offset_m: Sequence[float] = (0.0, 0.0, 0.0),
|
||||
root: Optional[Path] = None,
|
||||
) -> Tuple[sim_scene.SceneSpec, TruthTarget]:
|
||||
"""读场景 + 解出真值目标。返回 ``(场景, 目标)``，场景用于启动日志。
|
||||
|
||||
``object_name`` 空 = 用场景里的 ``operating.part_object``：**"哪个是工件"
|
||||
是场景的知识，不是流程的知识**，所以默认跟着场景走，换场景不用改流程参数。
|
||||
"""
|
||||
spec = sim_scene.load_scene(scene_file, root)
|
||||
return spec, target_point_world(spec, pick_object(spec, object_name), offset_m, root)
|
||||
|
||||
|
||||
def apply_transform(T: np.ndarray, p: Sequence[float]) -> Tuple[float, float, float]:
|
||||
"""4×4 齐次变换作用于点。"""
|
||||
q = np.asarray(T, dtype=float) @ np.array([p[0], p[1], p[2], 1.0], dtype=float)
|
||||
return (float(q[0]), float(q[1]), float(q[2]))
|
||||
|
||||
|
||||
def matrix_from_transform(tr) -> np.ndarray:
|
||||
"""``TransformStamped`` → 4×4 齐次矩阵。
|
||||
|
||||
四元数→旋转矩阵**不在这里实现**：复用 ``handeye_lib.quat_xyzw_to_R``
|
||||
（那是全仓库唯一一份，且有回环测试钉着）。重复实现的下场见模块 docstring。
|
||||
"""
|
||||
from arm_application.handeye_lib import quat_xyzw_to_R
|
||||
|
||||
t = tr.transform.translation
|
||||
q = tr.transform.rotation
|
||||
T = np.eye(4, dtype=float)
|
||||
T[:3, :3] = quat_xyzw_to_R([q.x, q.y, q.z, q.w])
|
||||
T[0, 3] = float(t.x)
|
||||
T[1, 3] = float(t.y)
|
||||
T[2, 3] = float(t.z)
|
||||
return T
|
||||
|
||||
|
||||
def pose_drift(
|
||||
obj: sim_scene.SceneObject,
|
||||
p: Sequence[float],
|
||||
quat: Sequence[float],
|
||||
tol_m: float,
|
||||
tol_deg: float,
|
||||
) -> Optional[str]:
|
||||
"""实际位姿相对名义位姿的偏差说明；在容差内返回 ``None``（= 没漂移）。
|
||||
|
||||
**纯函数**（不碰 ROS、不碰 TF）：这条判据搞错了的表现是"监视器静默失声" ——
|
||||
要么永远不报（物体明明被撞走了却没人说），要么天天误报（现场学会忽略它）。
|
||||
两种情况都比没有监视更糟，所以它必须能脱离节点被钉住。
|
||||
"""
|
||||
from arm_application.handeye_lib import quat_xyzw_to_R
|
||||
|
||||
d = math.dist((float(obj.x), float(obj.y), float(obj.z)), [float(v) for v in p[:3]])
|
||||
R_now = quat_xyzw_to_R([float(v) for v in quat])
|
||||
yaw_now = math.atan2(float(R_now[1, 0]), float(R_now[0, 0]))
|
||||
# 姿态用**整段旋转差**的夹角，不只比 yaw：板被碰一下往往是"翘起来夹着转了一点"，
|
||||
# 只比 yaw 会漏掉翘起，而翘起恰恰最影响孔的成像位置。
|
||||
c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw))
|
||||
R_ref = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
|
||||
cosang = (float(np.trace(R_ref.T @ R_now)) - 1.0) / 2.0
|
||||
ang = math.degrees(math.acos(max(-1.0, min(1.0, cosang))))
|
||||
if d <= tol_m and ang <= tol_deg:
|
||||
return None
|
||||
return (
|
||||
f"名义 (x,y,z)=({obj.x:.4f},{obj.y:.4f},{obj.z:.4f}) "
|
||||
f"yaw={math.degrees(float(obj.yaw)):.1f}°\n"
|
||||
f" 实际 (x,y,z)=({p[0]:.4f},{p[1]:.4f},{p[2]:.4f}) "
|
||||
f"yaw={math.degrees(yaw_now):.1f}°\n"
|
||||
f" 偏差 |Δp|={d * 1000:.1f}mm、姿态差 {ang:.1f}°"
|
||||
f"（容差 {tol_m * 1000:.1f}mm / {tol_deg:.1f}°）"
|
||||
)
|
||||
|
||||
|
||||
class SimTruthPerception(Node):
|
||||
"""把真值目标点按固定频率发到 ``visual_servo/filtered_point``。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim_truth_perception")
|
||||
self.declare_parameter("scene_file", "")
|
||||
"""场景文件（空 = 仓库根下 devices/sim/gazebo/config/scene.yaml）。"""
|
||||
|
||||
self.declare_parameter("target_object", "")
|
||||
"""目标物体名（空 = 场景里的 operating.part_object）。"""
|
||||
|
||||
self.declare_parameter("offset_m", [0.0, 0.0, 0.0])
|
||||
"""物体局部系偏移 [m]，例如挪到某个孔：``[0.0, -0.029, 0.0]``。"""
|
||||
|
||||
self.declare_parameter("camera_frame", "")
|
||||
"""相机光学系（由 launch 按当前臂/相机传进来，不写死）。"""
|
||||
|
||||
self.declare_parameter("world_frame", "world")
|
||||
"""场景的世界系。Gazebo 里模型位姿就是相对它，TF 树上也有。"""
|
||||
|
||||
self.declare_parameter("point_topic", "visual_servo/filtered_point")
|
||||
self.declare_parameter("publish_hz", 20.0)
|
||||
"""发布频率：不必跟着相机帧率，伺服 100Hz 控制会自己插值/外推。"""
|
||||
|
||||
self.declare_parameter("pose_info_topic", "")
|
||||
"""世界模型位姿话题（``tf2_msgs/TFMessage``）。
|
||||
|
||||
空 = 用 scene.yaml 的 ``pose_info.ros_topic``；场景里也没配 = **不做**
|
||||
"物体被撞走"的监视（节点仍能发点，只是失去了前提自检）。"""
|
||||
|
||||
scene_file = str(self.get_parameter("scene_file").value or "").strip()
|
||||
self._scene_file = Path(scene_file) if scene_file else None
|
||||
offset = self.get_parameter("offset_m").value or [0.0, 0.0, 0.0]
|
||||
# 长度的校验在 target_point_world 里（纯函数，有测试钉着），这里不重复。
|
||||
self._camera_frame = str(self.get_parameter("camera_frame").value or "").strip()
|
||||
if not self._camera_frame:
|
||||
raise TruthError(
|
||||
"camera_frame 没给。它由 visual_servo.launch.py 按当前臂的相机传进来"
|
||||
"（site.yaml → cameras/ perception.camera_frame）。"
|
||||
"手工跑这个节点时必须显式指定，例如 "
|
||||
"--ros-args -p camera_frame:=sim_d405_color_optical_frame"
|
||||
)
|
||||
self._world_frame = str(self.get_parameter("world_frame").value or "world").strip()
|
||||
|
||||
# 目标点在启动时算一次就够：场景是静态的（工件被推动后位置会变，
|
||||
# 但那种情况下"真值"本身也就失效了 —— 此时应该重跑 gen_sim_handeye
|
||||
# 或重启，而不是悄悄跟着一个被撞歪的位姿走）。
|
||||
# ⚠️ 这句话在 2026-09-24 之前只是**注释**：工件真的被碰走了，没有任何人
|
||||
# 发现（详见 scene.yaml 的 pose_info 段）。现在由 _setup_drift_watch
|
||||
# 把它变成**运行时的判据**：一旦对上不，立刻停发并指名报错。
|
||||
self._spec, self._target = load_truth_target(
|
||||
self._scene_file,
|
||||
str(self.get_parameter("target_object").value or ""),
|
||||
offset,
|
||||
)
|
||||
|
||||
self._pub = self.create_publisher(
|
||||
PointStamped, str(self.get_parameter("point_topic").value), POINT_QOS
|
||||
)
|
||||
self._tf = Buffer()
|
||||
TransformListener(self._tf, self, spin_thread=True)
|
||||
self._sent = 0
|
||||
self._warned_tf = 0
|
||||
self._drifted: Dict[str, str] = {}
|
||||
self._warned_drift = 0
|
||||
self._watch: Dict[str, sim_scene.SceneObject] = {}
|
||||
self._pose_seen = 0
|
||||
self._t_start: Optional[float] = None
|
||||
self._setup_drift_watch()
|
||||
|
||||
hz = max(1.0, min(100.0, float(self.get_parameter("publish_hz").value)))
|
||||
self.create_timer(1.0 / hz, self._tick)
|
||||
self.get_logger().warn(
|
||||
"真值感知已启动（仿真专用）：不跑任何推理，直接把已知位姿当测量发出去。\n"
|
||||
f" 场景 {self._spec.source}\n"
|
||||
f" 目标 {self._target.note}\n"
|
||||
f" 世界系点 ({self._target.world_xyz[0]:.3f}, "
|
||||
f"{self._target.world_xyz[1]:.3f}, {self._target.world_xyz[2]:.3f}) "
|
||||
f"→ 相机系 {self._camera_frame}（TF: {self._world_frame} → "
|
||||
f"{self._camera_frame}） 频率 {hz:.0f}Hz\n"
|
||||
f" ⚠️ 它验证的是控制与接口，不是检测算法 —— 见 "
|
||||
f"devices/sim/gazebo/README.md「仿真里哪些模块用真值替代」"
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 漂移监视：真值感知的"前提自检"
|
||||
# ------------------------------------------------------------------
|
||||
def _setup_drift_watch(self) -> None:
|
||||
"""订阅世界模型位姿，盯住"被真值当作已知"的物体有没有被撞走。
|
||||
|
||||
为什么这条监视属于真值感知、而不是额外功能：
|
||||
真值感知的**前提**是"物体就在 scene.yaml 写的位置"。前提一旦破，
|
||||
算出来的就是假测量，而且不会报错 —— 现象全指向别处（见模块 docstring
|
||||
与 scene.yaml 的 pose_info 段）。所以判据必须由假设这一侧自己盯着。
|
||||
|
||||
填 ``self._watch``（``名字 → SceneObject``）；空 = 不监视。
|
||||
"""
|
||||
pi = self._spec.pose_info
|
||||
topic = str(self.get_parameter("pose_info_topic").value or "").strip()
|
||||
if not topic and pi is not None:
|
||||
topic = pi.ros_topic
|
||||
if not topic:
|
||||
self.get_logger().info(
|
||||
"没有世界模型位姿话题（scene.yaml 的 pose_info 未配）—— "
|
||||
"跳过「物体被撞走」的前提自检。真值点照发，但物体被碰走后"
|
||||
"不会有人报错（现象会变成 S05 孔距不对 / S07 不接触）。"
|
||||
)
|
||||
return
|
||||
|
||||
# 只盯**工件**：它是二阶段孔位与伺服目标的几何基准。
|
||||
# 通止规刻意不盯 —— 它本来就要被夹走、被搬动，那正是流程的一部分。
|
||||
want = self._spec.operating.part_object
|
||||
obj = next((o for o in self._spec.objects if o.name == want), None)
|
||||
if obj is None:
|
||||
self.get_logger().warn(
|
||||
f"scene.yaml 的 operating.part_object={want!r} 不在 objects 里 —— "
|
||||
f"漂移监视无从下手（没有名义位姿可比）。"
|
||||
)
|
||||
return
|
||||
|
||||
self._tol_m = float(pi.drift_tol_m if pi is not None else 0.002)
|
||||
self._tol_deg = float(pi.drift_tol_deg if pi is not None else 2.0)
|
||||
self.create_subscription(TFMessage, topic, self._on_pose_info, 10)
|
||||
self._watch = {obj.name: obj}
|
||||
self.get_logger().info(
|
||||
f"已盯住 {obj.name!r} 的实际位姿（{topic}）：偏离 "
|
||||
f"({obj.x:.3f},{obj.y:.3f},{obj.z:.3f}) yaw={math.degrees(obj.yaw):.1f}° "
|
||||
f"超过 {self._tol_m * 1000:.1f}mm / {self._tol_deg:.1f}° 就停发并报错。"
|
||||
)
|
||||
|
||||
def _on_pose_info(self, msg: TFMessage) -> None:
|
||||
"""``/world/<world>/pose/info`` → 每个模型的 ``child_frame_id`` 就是模型名。"""
|
||||
self._pose_seen += 1
|
||||
for tr in msg.transforms:
|
||||
name = str(tr.child_frame_id or "").strip().lstrip("/")
|
||||
obj = self._watch.get(name)
|
||||
if obj is None:
|
||||
continue
|
||||
t = tr.transform.translation
|
||||
q = tr.transform.rotation
|
||||
reason = pose_drift(
|
||||
obj,
|
||||
(float(t.x), float(t.y), float(t.z)),
|
||||
(float(q.x), float(q.y), float(q.z), float(q.w)),
|
||||
self._tol_m,
|
||||
self._tol_deg,
|
||||
)
|
||||
self._drifted[name] = reason or ""
|
||||
|
||||
def _tick(self) -> None:
|
||||
# 前提自检的**活性检查**：订阅了话题却一帧都没收到，等于没有监视 ——
|
||||
# 而那正是"以为有保护、实际裸奔"。所以这里必须把这种情况说出来。
|
||||
if self._watch and self._pose_seen == 0:
|
||||
if self._t_start is None:
|
||||
self._t_start = self.get_clock().now().nanoseconds * 1e-9
|
||||
elif self.get_clock().now().nanoseconds * 1e-9 - self._t_start > 8.0:
|
||||
self.get_logger().warn(
|
||||
"盯物体位姿的话题**一帧都没收到** —— 前提自检形同不存在。\n"
|
||||
" 常见原因：gazebo_sim.launch.py 里的 pose_info_bridge 没起来，"
|
||||
"或 scene.yaml 的 pose_info.gz_topic 与 world 名对不上\n"
|
||||
" （必须等于 /world/<world>/pose/info，world 名 = "
|
||||
"worlds/assembly.world 的文件名）。",
|
||||
throttle_duration_sec=10.0,
|
||||
)
|
||||
# 物体被撞走 → **不发点**。这条必须在最前面：它比"目标在不在相机前方"
|
||||
# 更根本 —— 目标本身就是按错的位姿算出来的，发出去就是幻影目标。
|
||||
if self._drifted:
|
||||
bad = {k: v for k, v in self._drifted.items() if v}
|
||||
if bad:
|
||||
name, reason = next(iter(bad.items()))
|
||||
self._warned_drift += 1
|
||||
if self._warned_drift in (1, 5, 30) or self._warned_drift % 300 == 0:
|
||||
self.get_logger().error(
|
||||
f"仿真物体 {name!r} 已经不在 scene.yaml 说的地方了 —— "
|
||||
f"**停止发真值点**（发出去就是幻影目标）。\n"
|
||||
f" {reason}\n"
|
||||
f" ⚠ 为什么必须停：真值感知是**纯几何**的，它按名义位姿算"
|
||||
f"孔位与目标点。\n"
|
||||
f" 物体一挪，它算出来的就是假测量，而且不会报错 ——"
|
||||
f"现象会变成\n"
|
||||
f" 「S05 两孔间距不像 k1/k2（169mm，真值 58mm）」"
|
||||
f"或「S07 一路下压不接触」，\n"
|
||||
f" 看起来像检测/标定/阈值坏了。根因只有这一条。\n"
|
||||
f" 修法：把 {name!r} 放回 scene.yaml 的位置（真机现场就改"
|
||||
f"scene.yaml），或重启仿真让它按场景重新 spawn。\n"
|
||||
f" 排查提示：台面沿 X 有一道 36mm 通槽（|y| ≤ 18mm）；"
|
||||
f"工件若跨槽时被压翻就会掉下去。"
|
||||
)
|
||||
return
|
||||
try:
|
||||
tr = self._tf.lookup_transform(self._camera_frame, self._world_frame, Time())
|
||||
except Exception as exc: # noqa: BLE001 —— tf2 的异常类型很杂，统一当"还没有"
|
||||
# 前几拍必然查不到（TF 要等 robot_state_publisher 起来），
|
||||
# 所以只刷限流告警，不报错。
|
||||
self._warned_tf += 1
|
||||
if self._warned_tf in (1, 20, 100) or self._warned_tf % 200 == 0:
|
||||
self.get_logger().warn(
|
||||
f"还查不到 TF {self._world_frame} → {self._camera_frame}"
|
||||
f"（已试 {self._warned_tf} 次）：{exc}",
|
||||
throttle_duration_sec=5.0,
|
||||
)
|
||||
return
|
||||
p = apply_transform(matrix_from_transform(tr), self._target.world_xyz)
|
||||
if p[2] < MIN_CAM_Z_M:
|
||||
# 目标在相机背后或贴到镜头上：不发！发出去伺服会朝反方向走。
|
||||
self.get_logger().warn(
|
||||
f"真值目标不在相机前方（相机系 z={p[2] * 1000:.0f}mm < "
|
||||
f"{MIN_CAM_Z_M * 1000:.0f}mm）—— 不发点。"
|
||||
f"世界系点 ({self._target.world_xyz[0]:.3f}, "
|
||||
f"{self._target.world_xyz[1]:.3f}, {self._target.world_xyz[2]:.3f})"
|
||||
f" 不在 {self._camera_frame} 视野里；先让臂摆到能看到它的位置。",
|
||||
throttle_duration_sec=5.0,
|
||||
)
|
||||
return
|
||||
|
||||
msg = PointStamped()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.header.frame_id = self._camera_frame
|
||||
msg.point.x, msg.point.y, msg.point.z = p
|
||||
self._pub.publish(msg)
|
||||
self._sent += 1
|
||||
if self._sent == 1:
|
||||
self.get_logger().info(
|
||||
f"首个真值目标点已发出：相机系 ({p[0]:.4f}, {p[1]:.4f}, {p[2]:.4f}) m"
|
||||
)
|
||||
elif self._sent % 200 == 0:
|
||||
self.get_logger().debug(f"已发 {self._sent} 个真值点")
|
||||
|
||||
|
||||
def main(argv=None) -> int:
|
||||
import traceback
|
||||
|
||||
rclpy.init(args=argv)
|
||||
node = None
|
||||
try:
|
||||
node = SimTruthPerception()
|
||||
rclpy.spin(node)
|
||||
return 0
|
||||
except TruthError as exc:
|
||||
# 配置错：把原因原样打出来（它是给人看的），别让 ROS 的异常栈盖住。
|
||||
print(f"真值感知起不来：{exc}", flush=True)
|
||||
return 2
|
||||
except Exception: # noqa: BLE001
|
||||
traceback.print_exc()
|
||||
return 1
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,159 @@
|
||||
"""仿真腕部力扭矩（wrist force/torque）的配置与桥接规则。
|
||||
|
||||
这一层只做两件事，别的都不做：
|
||||
|
||||
1. 从 ``devices/sim/gazebo/config/wrist_ft.yaml`` 读话题与关节名
|
||||
2. 生成 ros_gz_bridge 的参数（gz 消息 → ROS 消息）
|
||||
|
||||
**为什么不需要适配器节点**（与触觉不同）：
|
||||
触觉必须有 ``sim_tactile_adapter``，因为要做的语义工作很多 —— 轴向映射、
|
||||
基线相减、单位换算、接触超时、把 ``Contacts`` 折算成 contact 标志。
|
||||
腕部 FT 不需要任何这些：Gazebo 的 force_torque 直接给的就是**牛顿**，
|
||||
坐标系是 ``child`` 连杆系（工具竖直时 z 即轴向），而 ros_gz_bridge
|
||||
**原生支持** gz.msgs.Wrench → geometry_msgs/msg/WrenchStamped（实测确认）。
|
||||
于是仿真的输出与真机 ``/<sn>/ft_sensor_wrench`` **同名同类型同单位**，
|
||||
应用层一行分支都不用写。
|
||||
|
||||
少一层就少一个"配错了不报错"的地方，所以这里刻意不加节点。
|
||||
|
||||
与真机的对应：
|
||||
真机 /<sn>/ft_sensor_wrench geometry_msgs/WrenchStamped（法兰系，牛顿）
|
||||
仿真 /sim/ft_sensor_wrench geometry_msgs/WrenchStamped（link7 系，牛顿）
|
||||
← 名字由 site.yaml 的 robot.arms[sim].sn 决定
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Dict, List, Optional
|
||||
|
||||
import yaml
|
||||
|
||||
#: 相对仓库根的默认配置路径。
|
||||
DEFAULT_WRIST_FT_REL = "devices/sim/gazebo/config/wrist_ft.yaml"
|
||||
|
||||
|
||||
class WristFtConfigError(RuntimeError):
|
||||
"""腕部 FT 配置缺失或写错。消息里必须给出修法（现场就靠这句话）。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class WristFtSpec:
|
||||
"""腕部 FT 的一份完整描述。"""
|
||||
|
||||
topic: str
|
||||
joint: str
|
||||
ros_type: str
|
||||
gz_type: str
|
||||
publish_hz: float
|
||||
|
||||
|
||||
def load_wrist_ft(
|
||||
path: Optional[Path] = None, root: Optional[Path] = None
|
||||
) -> WristFtSpec:
|
||||
"""读腕部 FT 配置。找不到或写错时抛 WristFtConfigError，消息里给出修法。"""
|
||||
if path is None:
|
||||
base = root if root is not None else Path.cwd()
|
||||
path = Path(base) / DEFAULT_WRIST_FT_REL
|
||||
path = Path(path)
|
||||
|
||||
if not path.is_file():
|
||||
raise WristFtConfigError(
|
||||
f"腕部 FT 配置不存在: {path}\n"
|
||||
f" 它声明了仿真腕部力扭矩的话题名与挂载关节（桥接从它派生）。\n"
|
||||
f" 缺了它，S07 的接触判据在仿真里永远没有数据源。\n"
|
||||
f" 确认仓库根下存在 {DEFAULT_WRIST_FT_REL}。"
|
||||
)
|
||||
|
||||
try:
|
||||
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except yaml.YAMLError as exc:
|
||||
raise WristFtConfigError(
|
||||
f"腕部 FT 配置 YAML 解析失败: {path}\n 原因: {exc}"
|
||||
) from exc
|
||||
|
||||
if not isinstance(doc, dict):
|
||||
raise WristFtConfigError(f"{path} 的顶层应该是映射（key: value 形式）。")
|
||||
|
||||
missing = [
|
||||
k for k in ("topic", "joint", "ros_type", "gz_type") if not str(doc.get(k, "")).strip()
|
||||
]
|
||||
if missing:
|
||||
raise WristFtConfigError(
|
||||
f"{path} 缺字段: {', '.join(missing)}。\n"
|
||||
f" topic 必须与 arm.sim.sensors.xacro 里 wrist_ft_topic 的默认值\n"
|
||||
f" 完全一致，也要与应用层按 sn 推出的名字一致 —— 对不上时桥接器只是\n"
|
||||
f" 订阅了一个不存在的话题，不报错，而 S07 会一直判不到接触。"
|
||||
)
|
||||
|
||||
try:
|
||||
hz = float(doc.get("publish_hz", 100.0))
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise WristFtConfigError(
|
||||
f"{path} 的 publish_hz 不是数字: {doc.get('publish_hz')!r}"
|
||||
) from exc
|
||||
if hz <= 0.0:
|
||||
raise WristFtConfigError(f"{path} 的 publish_hz 必须为正，收到 {hz}")
|
||||
|
||||
return WristFtSpec(
|
||||
topic=str(doc["topic"]).strip(),
|
||||
joint=str(doc["joint"]).strip(),
|
||||
ros_type=str(doc["ros_type"]).strip(),
|
||||
gz_type=str(doc["gz_type"]).strip(),
|
||||
publish_hz=hz,
|
||||
)
|
||||
|
||||
|
||||
def bridge_args(spec: WristFtSpec) -> List[str]:
|
||||
"""生成 ros_gz_bridge parameter_bridge 的话题规则。
|
||||
|
||||
只做**传输**：gz 类型 → ROS 类型，话题名两侧保持同名。
|
||||
这条规则的终点就是应用层真正订阅的那个话题，所以话题名必须一次写对 ——
|
||||
桥接器没有"改名"能力，写错只会得到"话题上没数据"。
|
||||
"""
|
||||
return [f"{spec.topic}@{spec.ros_type}[{spec.gz_type}"]
|
||||
|
||||
|
||||
def check_wrist_ft_sensor(
|
||||
urdf_text: str, spec: WristFtSpec
|
||||
) -> List[str]:
|
||||
"""描述级静态检查：URDF 里到底有没有那个腕部 FT 传感器。
|
||||
|
||||
为什么需要（与 ``check_tactile_collisions`` 同一个理由）：
|
||||
Gazebo 对**挂错关节**的传感器不报错 —— 传感器会被静默丢弃，
|
||||
话题永远没有数据，现象是"S07 判不到接触"。光看 launch 日志完全看不出来
|
||||
（桥接器和 Gazebo 都说一切正常）。这里在启动时就把它指出来。
|
||||
|
||||
返回告警列表（空 = 没问题）；**不抛异常** —— 缺了它仿真仍能跑，
|
||||
只是 S07 的判据要退回触觉兜底，那是降级不是崩溃。
|
||||
"""
|
||||
warnings: List[str] = []
|
||||
if not urdf_text:
|
||||
return ["URDF 文本为空，无法核对腕部 FT 传感器"]
|
||||
|
||||
if f'reference="{spec.joint}"' not in urdf_text:
|
||||
near = [ln for ln in urdf_text.splitlines() if "gazebo reference=" in ln and "joint" in ln]
|
||||
warnings.append(
|
||||
f"URDF 里没有 <gazebo reference=\"{spec.joint}\"> —— "
|
||||
f"腕部 FT 传感器挂错关节了，它会被 Gazebo 静默丢弃、话题永远没数据。\n"
|
||||
f" 期望挂在 {spec.joint}（最后一个 revolute 关节：它下游挂着整个腕部以下"
|
||||
f" 的载荷，这才是工程意义上的腕部 FT）。\n"
|
||||
f" 注意不能挂 fixed 关节（如 flange_joint）——物理引擎不把它当独立关节"
|
||||
f" 求解，传感器永远没输出且不报错。\n"
|
||||
f" 当前 URDF 里的 <gazebo reference> 关节有: {', '.join(sorted(set(near))) or '（无）'}"
|
||||
)
|
||||
|
||||
if spec.topic not in urdf_text:
|
||||
warnings.append(
|
||||
f"URDF 里找不到腕部 FT 的话题 {spec.topic} —— 传感器的话题名与配置不一致。\n"
|
||||
f" 两侧必须一致：config 的 topic、arm.sim.sensors.xacro 的 wrist_ft_topic、\n"
|
||||
f" 以及应用层按 sn 推出的 /{{sn}}/ft_sensor_wrench（仿真 sn = sim）。"
|
||||
)
|
||||
|
||||
if "force_torque" not in urdf_text:
|
||||
warnings.append(
|
||||
"URDF 里没有任何 type=\"force_torque\" 传感器 —— 腕部 FT 不会被创建。"
|
||||
)
|
||||
|
||||
return warnings
|
||||
@@ -0,0 +1,247 @@
|
||||
#!/usr/bin/env python3
|
||||
"""真值孔位：把场景里**已知的孔几何**投影成二阶段要的那两个像素点。
|
||||
|
||||
为什么需要它
|
||||
------------
|
||||
二阶段（S05 精拍 → 对准）原本的输入是 ``hole_detect_hough`` 在图上找**亮环**。
|
||||
那条路要求工件检测模型先给出工件框（ROI），否则连"找哪儿"都不知道。
|
||||
|
||||
仿真的处境是：合成图过不了真机训练出来的工件模型（实测最高分 0.017），
|
||||
于是 S05 只能退回"整幅画面当 ROI"—— 而整幅画面里有**夹爪、通止规、台面**，
|
||||
HoughCircles 会在那儿找到"看起来像一对孔"的东西（实测两孔间距 108mm，
|
||||
真值是 58mm），随后被间距门槛丢掉，每拍都丢，最后超时。
|
||||
|
||||
真值路径直接把"孔在图上应该在哪"算出来：
|
||||
|
||||
孔世界点 = 物体世界位姿（scene.yaml）+ 环在物体局部系的位置（模型 <visual>）
|
||||
孔像素 = K · T_cam_world · 孔世界点
|
||||
|
||||
它验证的是**控制那一半**（坐标变换 / 手眼 / 伺服收敛 / 流程编排），
|
||||
不是感知那一半 —— 与 ``sim_truth_perception`` 同一条设计（见
|
||||
``devices/sim/gazebo/README.md`` 的「仿真里哪些模块用真值替代」）。
|
||||
|
||||
绝不做的事
|
||||
----------
|
||||
* 孔在相机背后（z<=0）时**不返回**结果。给伺服一个幻影目标会让机械臂朝错误
|
||||
方向走，而且看起来像"标定坏了" —— 和 ``sim_truth_perception`` 同一条规矩。
|
||||
* 不自己算四元数→旋转矩阵：T 由调用方（节点）用 TF 查好传进来。
|
||||
本模块只做**纯几何**，因此在没有 ROS 的环境里也能测。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
from pathlib import Path
|
||||
from typing import List, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
from arm_sim_gazebo import sim_scene
|
||||
|
||||
#: 孔（环）离相机光心太近或跑到背后就不返回：z 太小除法会放大误差，
|
||||
#: z<=0 在相机系里就是"背后"。取 2cm，与真值感知节点的取值同量级。
|
||||
MIN_CAM_Z_M = 0.02
|
||||
|
||||
#: 两种孔排序的判据：图像上 |dx| 大于这个比例倍数就按 x 排，否则按 y 排。
|
||||
#: 与 ``hole_detect_hough._detect_insert_pair`` 的规则一致（k1=左/上，
|
||||
#: k2=右/下）—— 换后端不该换语义，否则"哪边是 k1"会随环境变。
|
||||
_PAIR_HORIZONTAL_RATIO = 1.0
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class HoleTruth:
|
||||
"""一次真值定位的结果（``ok=False`` 时只有 ``why`` 有意义）。"""
|
||||
|
||||
ok: bool
|
||||
why: str = ""
|
||||
hole_points: List[Tuple[float, float]] = field(default_factory=list)
|
||||
#: 两孔的像素半径（环的物理半径按透视缩放；Hough 路径给的是它自己的估计，
|
||||
#: 真值路径给它一个**可信的**值 —— 上下游只把它当"尺度参考"用）
|
||||
hole_radii: List[float] = field(default_factory=list)
|
||||
#: 两孔**环心的世界坐标**（米），与 ``hole_points`` 一一对应、同序。
|
||||
#:
|
||||
#: 为什么真值路径要把三维也带出来（2026-09-24 的教训）：
|
||||
#: 原来只给像素，调用方（stage2）再拿**深度图**反投影回三维。仿真里
|
||||
#: 那一步是错的，而且错法很隐蔽：孔是**贯通孔**，孔位正下方就是台面
|
||||
#: 通槽 —— 深度相机在孔位看到的是**槽底/地面**（实测 0.62m），
|
||||
#: 而工件面其实只有 0.21m。拿 0.21m 的像素尺度去乘 0.62m 的深度，
|
||||
#: 三角化出的 k1/k2 间距变成 169mm（真值 58mm），被间距门槛丢掉。
|
||||
#: 真值路径**本来就知道**孔在世界系的精确位置，没有理由再绕一圈深度图。
|
||||
hole_world: List[Tuple[float, float, float]] = field(default_factory=list)
|
||||
#: 工件**上表面中心**的世界坐标（粗居中用；同样是"真值就该直接用"）。
|
||||
part_center_world: Optional[Tuple[float, float, float]] = None
|
||||
#: 工件上表面的世界 z（= 二阶段的对孔基准面）。
|
||||
part_top_z: Optional[float] = None
|
||||
#: 工件在画面里占的框（孔检测的 ROI 用；也是"工件中心"的来源）
|
||||
bbox: Tuple[int, int, int, int] = (0, 0, 0, 0)
|
||||
#: 给人看的说明（进了日志才知道这个数是哪来的）
|
||||
note: str = ""
|
||||
|
||||
|
||||
def _object_world_xy(obj: sim_scene.SceneObject, lx: float, ly: float) -> Tuple[float, float]:
|
||||
"""物体局部 (x,y) → 世界 (x,y)（含物体自身 yaw）。"""
|
||||
c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw))
|
||||
return (float(obj.x) + c * lx - s * ly, float(obj.y) + s * lx + c * ly)
|
||||
|
||||
|
||||
def project_point(
|
||||
T_cam_world: np.ndarray, intr: Sequence[float], xyz: Sequence[float]
|
||||
) -> Optional[Tuple[float, float, float]]:
|
||||
"""世界点 → ``(u, v, z_cam)``；在相机背后/太近时返回 None。"""
|
||||
T = np.asarray(T_cam_world, dtype=float)
|
||||
p = T @ np.array([float(xyz[0]), float(xyz[1]), float(xyz[2]), 1.0], dtype=float)
|
||||
z = float(p[2])
|
||||
if not math.isfinite(z) or z <= MIN_CAM_Z_M:
|
||||
return None
|
||||
fx, fy, cx, cy = (float(intr[0]), float(intr[1]), float(intr[2]), float(intr[3]))
|
||||
return (fx * float(p[0]) / z + cx, fy * float(p[1]) / z + cy, z)
|
||||
|
||||
|
||||
def object_bbox_px(
|
||||
T_cam_world: np.ndarray,
|
||||
intr: Sequence[float],
|
||||
boxes: Sequence[Sequence[float]],
|
||||
image_shape: Tuple[int, int],
|
||||
) -> Optional[Tuple[int, int, int, int]]:
|
||||
"""物体各碰撞体世界 AABB 的角点投影 → 像素包围框（裁剪到画面内）。
|
||||
|
||||
返回 None = 一个角点都投不出来（跑到背后去了），调用方应当放弃这一拍。
|
||||
"""
|
||||
h, w = int(image_shape[0]), int(image_shape[1])
|
||||
us: List[float] = []
|
||||
vs: List[float] = []
|
||||
for b in boxes:
|
||||
for x in (b[0], b[1]):
|
||||
for y in (b[2], b[3]):
|
||||
for z in (b[4], b[5]):
|
||||
got = project_point(T_cam_world, intr, (x, y, z))
|
||||
if got is None:
|
||||
continue
|
||||
us.append(got[0])
|
||||
vs.append(got[1])
|
||||
if not us:
|
||||
return None
|
||||
x1 = max(0, min(w - 1, int(math.floor(min(us)))))
|
||||
y1 = max(0, min(h - 1, int(math.floor(min(vs)))))
|
||||
x2 = max(0, min(w - 1, int(math.ceil(max(us)))))
|
||||
y2 = max(0, min(h - 1, int(math.ceil(max(vs)))))
|
||||
if x2 <= x1 or y2 <= y1:
|
||||
return None
|
||||
return (x1, y1, x2, y2)
|
||||
|
||||
|
||||
def locate_holes(
|
||||
T_cam_world: Optional[np.ndarray],
|
||||
intr: Optional[Sequence[float]],
|
||||
image_shape: Tuple[int, int],
|
||||
*,
|
||||
spec: Optional[sim_scene.SceneSpec] = None,
|
||||
object_name: str = "",
|
||||
root: Optional[Path] = None,
|
||||
) -> HoleTruth:
|
||||
"""算这一拍的两个孔（像素 + ROI 框）。
|
||||
|
||||
``T_cam_world`` 为 None（TF 还没到）时返回 ``ok=False``，由调用方决定是
|
||||
继续用检测器还是报错 —— 本函数不猜、不兜底。
|
||||
"""
|
||||
if T_cam_world is None or intr is None:
|
||||
return HoleTruth(ok=False, why="还没有 T_cam_world / 相机内参（TF 或 camera_info 未到）")
|
||||
try:
|
||||
spec = spec if spec is not None else sim_scene.load_scene(None, root)
|
||||
except Exception as exc: # noqa: BLE001 —— 场景读不出来要说清是场景的问题
|
||||
return HoleTruth(ok=False, why=f"读不到场景配置：{exc}")
|
||||
want = (object_name or "").strip() or spec.operating.part_object
|
||||
obj = next((o for o in spec.objects if o.name == want), None)
|
||||
if obj is None:
|
||||
names = ", ".join(o.name for o in spec.objects) or "（空）"
|
||||
return HoleTruth(ok=False, why=f"场景里没有物体 {want!r}（现有：{names}）")
|
||||
rings = sim_scene.rings_local(spec, obj, root)
|
||||
if len(rings) != 2:
|
||||
return HoleTruth(
|
||||
ok=False,
|
||||
why=(
|
||||
f"工件 {obj.name!r} 的模型里有 {len(rings)} 个嵌件亮环，"
|
||||
f"真值孔位要求正好 2 个（模型 <visual name=\"*_ring\"> 的名字约定）"
|
||||
),
|
||||
)
|
||||
|
||||
pts: List[Tuple[float, float]] = []
|
||||
rad: List[float] = []
|
||||
holes_world: List[Tuple[float, float, float]] = []
|
||||
for ring in rings:
|
||||
wx, wy = _object_world_xy(obj, ring.x, ring.y)
|
||||
wz = float(obj.z) + float(ring.z)
|
||||
got = project_point(T_cam_world, intr, (wx, wy, wz))
|
||||
if got is None:
|
||||
return HoleTruth(
|
||||
ok=False,
|
||||
why=f"孔 {ring.name} 投影到相机背后（z<=0）—— 相机没对着工件时不该出结果",
|
||||
)
|
||||
u, v, z = got
|
||||
pts.append((u, v))
|
||||
rad.append(float(intr[0]) * float(ring.radius) / z)
|
||||
holes_world.append((wx, wy, wz))
|
||||
|
||||
# k1/k2 的左右（上下）规则与 Hough 路径一致：水平对按 x，竖直对按 y。
|
||||
(p, q) = pts
|
||||
horizontal = abs(p[0] - q[0]) >= _PAIR_HORIZONTAL_RATIO * abs(p[1] - q[1])
|
||||
key = 0 if horizontal else 1
|
||||
if pts[0][key] > pts[1][key]:
|
||||
pts = [pts[1], pts[0]]
|
||||
rad = [rad[1], rad[0]]
|
||||
holes_world = [holes_world[1], holes_world[0]]
|
||||
|
||||
boxes = sim_scene.world_boxes(spec, obj, root)
|
||||
bbox = object_bbox_px(T_cam_world, intr, boxes, image_shape)
|
||||
if bbox is None:
|
||||
return HoleTruth(ok=False, why="工件整体投影不出画面框（角点都在相机背后）")
|
||||
|
||||
# 工件上表面中心与上表面 z：从各碰撞体的世界 AABB 取（AABB 是轴对齐的，
|
||||
# 只有 yaw 的物体上它正好等于"外接矩形"，对"粗居中"这个用途是合适的）。
|
||||
part_center: Optional[Tuple[float, float, float]] = None
|
||||
part_top: Optional[float] = None
|
||||
if boxes:
|
||||
xs = [float(v) for b in boxes for v in (b[0], b[1])]
|
||||
ys = [float(v) for b in boxes for v in (b[2], b[3])]
|
||||
zs = [float(v) for b in boxes for v in (b[4], b[5])]
|
||||
part_top = max(zs)
|
||||
part_center = (0.5 * (min(xs) + max(xs)), 0.5 * (min(ys) + max(ys)), part_top)
|
||||
|
||||
dist_px = math.hypot(pts[0][0] - pts[1][0], pts[0][1] - pts[1][1])
|
||||
note = (
|
||||
f"{obj.name}（模型 {obj.model}） 环心距 {dist_px:.1f}px "
|
||||
f"环半径 {rad[0]:.1f}/{rad[1]:.1f}px 框 {bbox}"
|
||||
)
|
||||
return HoleTruth(
|
||||
ok=True,
|
||||
why="",
|
||||
hole_points=[(float(pts[0][0]), float(pts[0][1])), (float(pts[1][0]), float(pts[1][1]))],
|
||||
hole_radii=[float(rad[0]), float(rad[1])],
|
||||
hole_world=holes_world,
|
||||
part_center_world=part_center,
|
||||
part_top_z=part_top,
|
||||
bbox=bbox,
|
||||
note=note,
|
||||
)
|
||||
|
||||
|
||||
def draw(bgr: np.ndarray, res: HoleTruth) -> np.ndarray:
|
||||
"""把真值结果画在图上（叠图/日志用）。失败时只写一行原因。"""
|
||||
import cv2
|
||||
|
||||
img = bgr.copy()
|
||||
h, w = img.shape[:2]
|
||||
if not res.ok:
|
||||
cv2.putText(img, "TRUTH: n/a", (12, 28), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 0, 255), 2)
|
||||
return img
|
||||
x1, y1, x2, y2 = res.bbox
|
||||
cv2.rectangle(img, (x1, y1), (x2, y2), (0, 255, 255), 2)
|
||||
for i, (u, v) in enumerate(res.hole_points):
|
||||
r = int(round(res.hole_radii[i])) if i < len(res.hole_radii) else 8
|
||||
cv2.circle(img, (int(round(u)), int(round(v))), max(3, r), (0, 0, 255), 2)
|
||||
cv2.putText(
|
||||
img, f"T{i + 1}", (int(u) + 8, int(v)), cv2.FONT_HERSHEY_SIMPLEX, 0.60, (0, 0, 255), 2
|
||||
)
|
||||
cv2.putText(img, "TRUTH", (12, min(h - 8, 28)), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
|
||||
return img
|
||||
@@ -0,0 +1,501 @@
|
||||
"""把厂商给的 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 质量的问题。
|
||||
|
||||
问题三：被 mimic 的关节 effort="0"，在 Gazebo 里出不了力
|
||||
sdformat 转 SDF 时会**直接丢掉 <mimic>**（实测生成的 SDF 里
|
||||
"mimic" 出现 0 次），gz-sim 也没有任何 mimic 实现，SDF 规范里
|
||||
根本没有这个元素。于是夹爪那 6 个"靠机械耦合跟随"的手指关节
|
||||
在 Gazebo 里变成互相独立的关节，谁都不跟随谁。
|
||||
更麻烦的是它们的 effort="0"（上游语义：不由电机驱动）——
|
||||
而 gz_ros2_control 的 position 接口是"位置误差 → 力"，
|
||||
力被 effort 上限夹到 0，于是**永远出不了力**：
|
||||
接口 [claimed]、控制器 active、话题有订阅、命令也发得出去，
|
||||
但手指一动不动，只是自由地被重力拽着漂。
|
||||
详见 MIMIC_EFFORT_FIX 处的注释。
|
||||
|
||||
=============================================================================
|
||||
修法原则
|
||||
=============================================================================
|
||||
只做"重新分组 + 补极小量"，不发明任何物理数据，也不掩盖真问题：
|
||||
- 孤儿三件套 → 只是把已有元素包进 <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"
|
||||
|
||||
# 被 mimic 的关节在 Gazebo 里失去机械耦合，必须能自己出力，所以把 effort="0"
|
||||
# 补成这个值。取 80 Nm：与夹爪那个真正被驱动的 finger_width_joint 的 effort 一致
|
||||
# （grav_macro.xacro 里写的就是 80），所以不引入任何新的"凭空数据"，
|
||||
# 只是把"上游说这关节不由电机驱动"翻译成"仿真里它必须自己出力"。
|
||||
#
|
||||
# 为什么需要这条修复（实测，2026-09-23）：
|
||||
# 夹爪只有 finger_width_joint 是真关节（effort=80），其余 6 个手指关节
|
||||
# 全是 mimic、effort="0"（上游语义：它们靠机械耦合跟随，不由电机驱动）。
|
||||
# 而 sdformat **直接丢弃 <mimic>**，gz-sim 也不支持 mimic ——
|
||||
# 于是在 Gazebo 里它们是 7 个互相独立的关节，谁都不跟随谁。
|
||||
#
|
||||
# 症状极具误导性：命令下发成功（接口 [claimed]、控制器 active、话题有订阅），
|
||||
# 但手指一动不动，只是自由地被重力拽着漂。原因是 gz_ros2_control 的 position
|
||||
# 接口实现是"位置误差 → 力"（实测 position_proportional_gain=0.1），
|
||||
# 力被 effort 上限夹到 0，于是永远出不了力。
|
||||
#
|
||||
# 对照实验：同一套链路下 sim_joint1 收到 0.2 rad 的目标后精确到位（0.19998），
|
||||
# 所以控制链路本身没问题，差别只在这 6 个关节的 effort=0。
|
||||
MIMIC_EFFORT_FIX = "80"
|
||||
|
||||
# =============================================================================
|
||||
# 夹爪出力上限：把 gz_ros2_control 的"位置控制"夹成"力控夹爪"
|
||||
# =============================================================================
|
||||
# 为什么需要（实测，2026-09-24）：
|
||||
# gz_ros2_control 的 position 接口是"位置误差 → 力"，力只被 effort 上限夹。
|
||||
# grav_macro 给手指关节写的 effort=80（单位在这里是 Nm），而指腹到指节的
|
||||
# 力臂只有 ~0.1m —— 于是"合爪"能压出**上千牛**：仿真里 30mm 的规被两指
|
||||
# 直接挤穿（实测关节走到 0 位=开口 27mm < 规径 30mm），规被弹出去、掉回规座，
|
||||
# 触觉却报着上百牛的力。表现像"夹爪抓不住"，根因是出力没有上限。
|
||||
#
|
||||
# 取值怎么来的（不用猜：从 URDF 自己的行程推）：
|
||||
# 夹爪是平行四边形连杆，指腹平动 100mm 时手指关节转 0.9404 rad
|
||||
# （inner/outer_finger 的 limits：-0.7854..0.155），所以
|
||||
# 力臂 ≈ 行程 / 角行程 = 0.100 / 0.9404 ≈ 0.106 m
|
||||
# 真机 Grav 的握力档位是 20~50N，取 45N（覆盖应用层常用的 force=30）：
|
||||
# 手指关节 effort = 45 × 0.106 ≈ 4.8 Nm
|
||||
# 而 finger_width_joint 是 1:1 的"行程关节"（0..0.1m），它的 effort 就是握力
|
||||
# 本身，给 45（N）。
|
||||
#
|
||||
# ⚠ 这一条是**仿真专用**的物理量：真机的握力由 GripperExecute.force 走力控闭环，
|
||||
# 不靠关节 effort 上限。归一化只服务于 Gazebo，所以写在这里、不去动上游描述。
|
||||
GRIP_FORCE_N = 45.0
|
||||
GRIP_STROKE_M = 0.100
|
||||
GRIP_FINGER_TRAVEL_RAD = 0.9404
|
||||
GRIP_PAD_LEVER_M = GRIP_STROKE_M / GRIP_FINGER_TRAVEL_RAD # ≈ 0.1064 m
|
||||
GRIP_FINGER_EFFORT_NM = round(GRIP_FORCE_N * GRIP_PAD_LEVER_M, 2) # ≈ 4.79 Nm
|
||||
GRIP_WIDTH_EFFORT_N = GRIP_FORCE_N
|
||||
|
||||
#: 名字里带这些字样的关节 = 夹爪的指节/指腹/行程关节（仿真里统一带 sim_ 前缀）
|
||||
_GRIP_JOINT_HINTS = ("finger", "knuckle")
|
||||
#: 行程关节（1:1，effort 单位就是 N）
|
||||
_GRIP_WIDTH_HINT = "finger_width"
|
||||
|
||||
# =============================================================================
|
||||
# 为什么要把 <mimic> 整个从描述里删掉
|
||||
# =============================================================================
|
||||
# 除了上面的 effort 问题，<mimic> 留在描述里还有第二个隐患：
|
||||
#
|
||||
# robot_state_publisher **支持** mimic —— 它会用"主关节的值 × multiplier + offset"
|
||||
# 覆盖从动关节的位置，而不是用 /joint_states 里广播的真实值。
|
||||
# 而 Gazebo 里这些关节是独立被我们驱动的，真实值与 mimic 公式算出来的值
|
||||
# 在**我们没按公式驱动时**（或主关节没跟上时）就不一样。
|
||||
# 后果是 TF 与 Gazebo 各说各话，而且不报错。
|
||||
#
|
||||
# 实测（2026-09-23）：当前版本的 RSP **没有**应用 mimic（TF 跟随真实关节值），
|
||||
# 所以现在两边碰巧一致。但这是 RSP 的实现细节，不是契约 —— 它一变，
|
||||
# TF 就会静默退回闭合位。
|
||||
#
|
||||
# 因此归一化时把 <mimic> 删掉，让描述与 Gazebo 的事实一致：
|
||||
# "7 个独立关节，全部由我们显式驱动"。
|
||||
# 耦合关系（谁跟随谁、乘多少）由 sim_gripper_adapter 显式负责，
|
||||
# 它从**未归一化**的展开结果里读（见 robot_description.mimic_relations）。
|
||||
#
|
||||
# 删掉的信息不会丢：会记进 NormalizeReport.stripped_mimics 并被打印出来。
|
||||
# =============================================================================
|
||||
|
||||
# 裸露的 origin + mass + inertia 三件套。
|
||||
#
|
||||
# ⚠️ 标签之间的空白必须是**可选**的（\s*），不能要求有换行 —— 这是个实测踩到的坑：
|
||||
# xacro 的输出格式在不同环境里不一样。真机环境的 xacro 会缩进成多行：
|
||||
# <origin .../>
|
||||
# <mass .../>
|
||||
# <inertia .../>
|
||||
# 而仿真环境（另一份 xacro 版本）走 xacro.process_file().toxml() 时**全部挤在一行**：
|
||||
# <link ...><origin .../><mass .../><inertia .../><visual ...>
|
||||
# 早期这里的正则要求标签之间必须有 \n，于是 Python 路径下归一化静默地什么都没做
|
||||
# （报告"包裹 0 处"），而 CLI 路径下却正常 —— 因为命令行会缩进。
|
||||
# 后果极严重：Gazebo 会丢掉整条机械臂，正是这个归一化本来要防的事。
|
||||
# 教训：这段代码不能对空白格式有任何假设。
|
||||
#
|
||||
# 夹爪的 <inertial> 内部也是这三个标签，所以必须按 link 块处理并跳过已含
|
||||
# <inertial> 的块，否则会二次包裹 —— sdformat 会报
|
||||
# "Inertial element must have a mass element"（已踩过）。
|
||||
_TRIPLE = re.compile(
|
||||
r"(?P<ind>[ \t]*)<origin(?P<o>[^>]*?)/>"
|
||||
r"\s*<mass(?P<m>[^>]*?)/>"
|
||||
r"\s*<inertia(?P<i>[^>]*?)/>"
|
||||
)
|
||||
|
||||
_LINK_BLOCK = re.compile(r"<link\b[^>]*>.*?</link>", re.S)
|
||||
|
||||
_JOINT_BLOCK = re.compile(r"<joint\b[^>]*>.*?</joint>", re.S)
|
||||
_MIMIC_TAG = re.compile(r"<mimic\b")
|
||||
|
||||
# <limit effort="0" .../>：只认数值 0（或 0.0 / 0.00…），不碰其它值。
|
||||
_LIMIT_ZERO_EFFORT = re.compile(r"<limit\b(?P<attrs>[^>]*?)/>")
|
||||
|
||||
_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)
|
||||
fixed_zero_effort: List[str] = field(default_factory=list)
|
||||
#: 被删掉的 mimic 关系 (关节, 源关节, multiplier, offset)
|
||||
stripped_mimics: List[Tuple[str, str, str, str]] = field(default_factory=list)
|
||||
#: 被压到真机握力量级的夹爪关节 (关节, 原值, 新值)
|
||||
clamped_grip_effort: List[Tuple[str, str, 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
|
||||
or self.fixed_zero_effort
|
||||
or self.stripped_mimics
|
||||
or self.clamped_grip_effort
|
||||
)
|
||||
|
||||
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)} 个")
|
||||
if self.fixed_zero_effort:
|
||||
bits.append(
|
||||
f"被 mimic 关节补 effort={MIMIC_EFFORT_FIX} {len(self.fixed_zero_effort)} 个"
|
||||
f"({', '.join(self.fixed_zero_effort)})"
|
||||
)
|
||||
if self.stripped_mimics:
|
||||
bits.append(
|
||||
f"删除 mimic 关系 {len(self.stripped_mimics)} 条"
|
||||
f"（Gazebo 不支持 mimic，改由 sim_gripper_adapter 显式驱动）"
|
||||
)
|
||||
if self.clamped_grip_effort:
|
||||
bits.append(
|
||||
f"夹爪出力压到握力 {GRIP_FORCE_N:g}N 量级 {len(self.clamped_grip_effort)} 个"
|
||||
f"({', '.join(f'{n}:{a}→{b}' for n, a, b in self.clamped_grip_effort)})"
|
||||
)
|
||||
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 _fix_mimic_joint(block: str, report: NormalizeReport, joint_name: str) -> str:
|
||||
"""处理带 <mimic> 的关节：记下耦合关系、补 effort、删掉 mimic 标签。
|
||||
|
||||
为什么三件事一起做：它们都是同一个事实（"Gazebo 不支持 mimic"）的直接后果，
|
||||
分散在三处代码里迟早会有人只改一半。
|
||||
- 记下关系 → 适配器要知道谁跟随谁（信息不能丢，只是搬家）
|
||||
- 补 effort → 关节要能自己出力（见 MIMIC_EFFORT_FIX）
|
||||
- 删 mimic → 让描述与 Gazebo 的事实一致（见文件头那段说明）
|
||||
"""
|
||||
m = _MIMIC_TAG.search(block)
|
||||
if m is None:
|
||||
return block
|
||||
|
||||
# 1) 记录耦合关系（从 <mimic .../> 标签上抠出来）
|
||||
tag_end = block.find(">", m.start())
|
||||
tag = block[m.start() : tag_end + 1] if tag_end != -1 else ""
|
||||
attrs = _attrs_of(tag)
|
||||
report.stripped_mimics.append(
|
||||
(
|
||||
joint_name,
|
||||
attrs.get("joint", "?"),
|
||||
attrs.get("multiplier", "1"),
|
||||
attrs.get("offset", "0"),
|
||||
)
|
||||
)
|
||||
|
||||
# 2) 删掉 <mimic .../>（含它前面的缩进，免得留下孤零零的空白行）
|
||||
line_start = block.rfind("\n", 0, m.start()) + 1
|
||||
if block[line_start : m.start()].strip() == "":
|
||||
block = block[:line_start] + block[tag_end + 1 :].lstrip("\n")
|
||||
else:
|
||||
block = block[: m.start()] + block[tag_end + 1 :]
|
||||
|
||||
# 3) effort="0" → 可用值（只动这一个属性，限位是上游的物理事实，不碰）
|
||||
lm = _LIMIT_ZERO_EFFORT.search(block)
|
||||
if lm is not None:
|
||||
lattrs = _attrs_of(lm.group("attrs"))
|
||||
if _num(lattrs.get("effort", "")) == 0.0:
|
||||
report.fixed_zero_effort.append(joint_name)
|
||||
lattrs["effort"] = MIMIC_EFFORT_FIX
|
||||
new = "<limit " + " ".join(f'{k}="{v}"' for k, v in lattrs.items()) + "/>"
|
||||
block = block[: lm.start()] + new + block[lm.end() :]
|
||||
|
||||
return block
|
||||
|
||||
|
||||
def _clamp_grip_effort(block: str, report: NormalizeReport, joint_name: str) -> str:
|
||||
"""把夹爪关节的 effort 上限压到"真机握力量级"（见 GRIP_FORCE_N 的推导）。
|
||||
|
||||
为什么必须做：不做的话"合爪"能压出上千牛，把工件挤穿、弹飞，
|
||||
看起来像"夹爪抓不住"，实际是出力没有上限（详见 GRIP_FORCE_N 处注释）。
|
||||
|
||||
只改 effort 一个属性；限位、速度是上游的物理事实，一律不碰。
|
||||
"""
|
||||
low = joint_name.lower()
|
||||
if not any(h in low for h in _GRIP_JOINT_HINTS):
|
||||
return block
|
||||
want = GRIP_WIDTH_EFFORT_N if _GRIP_WIDTH_HINT in low else GRIP_FINGER_EFFORT_NM
|
||||
lm = _LIMIT_ZERO_EFFORT.search(block)
|
||||
if lm is None:
|
||||
return block
|
||||
lattrs = _attrs_of(lm.group("attrs"))
|
||||
cur = _num(lattrs.get("effort", ""))
|
||||
if cur <= want:
|
||||
return block # 上游本来就比我们更保守：尊重它，不往上调
|
||||
lattrs["effort"] = f"{want:g}"
|
||||
report.clamped_grip_effort.append((joint_name, f"{cur:g}", f"{want:g}"))
|
||||
new = "<limit " + " ".join(f'{k}="{v}"' for k, v in lattrs.items()) + "/>"
|
||||
return block[: lm.start()] + new + block[lm.end() :]
|
||||
|
||||
|
||||
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")
|
||||
o, m_, i = mm.group("o"), mm.group("m"), mm.group("i")
|
||||
# 原样保留输入的排版风格：多行输入就给多行输出，单行就给单行输出。
|
||||
# 凭空插入或删除换行会让 diff 里全是噪声，也不必要。
|
||||
if "\n" in mm.group(0):
|
||||
return (
|
||||
f"{ind}<inertial>\n"
|
||||
f"{ind} <origin{o}/>\n"
|
||||
f"{ind} <mass{m_}/>\n"
|
||||
f"{ind} <inertia{i}/>\n"
|
||||
f"{ind}</inertial>"
|
||||
)
|
||||
return f"{ind}<inertial><origin{o}/><mass{m_}/><inertia{i}/></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:])
|
||||
out = "".join(parts)
|
||||
|
||||
# --- 问题三：被 mimic 的关节 effort="0" ---
|
||||
# 放在 link 处理之后单独一遍：<joint> 不会嵌套在 <link> 里，顺序无关，
|
||||
# 而且这样不必把两套正则揉进同一个循环。
|
||||
def joint_repl(mm: re.Match) -> str:
|
||||
block = mm.group(0)
|
||||
jm = re.match(r'<joint\b[^>]*name\s*=\s*"([^"]*)"', block)
|
||||
name = jm.group(1) if jm else "?"
|
||||
if _MIMIC_TAG.search(block):
|
||||
block = _fix_mimic_joint(block, report, name)
|
||||
# 夹爪出力上限**放在 mimic 修复之后**：mimic 那条会把 effort=0 补成 80，
|
||||
# 而这里要把它压到真机握力的量级。顺序反了就等于没夹。
|
||||
return _clamp_grip_effort(block, report, name)
|
||||
|
||||
out = _JOINT_BLOCK.sub(joint_repl, out)
|
||||
return out, report
|
||||
|
||||
|
||||
def mimic_relations(urdf: str) -> List[Tuple[str, str, str, str]]:
|
||||
"""从（**未归一化的**）URDF 文本里读出所有 mimic 关系。
|
||||
|
||||
返回 [(关节, 源关节, multiplier, offset)]，数值以字符串给出（原样保留，
|
||||
由调用方决定怎么解释）。归一化会删掉 <mimic>，所以必须在归一化**之前**读 ——
|
||||
这也是这个函数放在本模块而不是适配器里的原因：谁删的谁负责提供读取入口。
|
||||
"""
|
||||
out: List[Tuple[str, str, str, str]] = []
|
||||
for m in _JOINT_BLOCK.finditer(urdf):
|
||||
block = m.group(0)
|
||||
mm_ = _MIMIC_TAG.search(block)
|
||||
if mm_ is None:
|
||||
continue
|
||||
jm = re.match(r'<joint\b[^>]*name\s*=\s*"([^"]*)"', block)
|
||||
tag_end = block.find(">", mm_.start())
|
||||
attrs = _attrs_of(block[mm_.start() : tag_end + 1] if tag_end != -1 else "")
|
||||
out.append(
|
||||
(
|
||||
jm.group(1) if jm else "?",
|
||||
attrs.get("joint", "?"),
|
||||
attrs.get("multiplier", "1"),
|
||||
attrs.get("offset", "0"),
|
||||
)
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
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())
|
||||
Reference in New Issue
Block a user