仿真感知：真值孔位 + 真值相机 + 补 /home，让流程不改代码就能跑仿真
三块补齐，目标是"同一个流程节点，仿真/真机都不改代码"：
1. 真值孔位（truth_holes.py）：把 scene.yaml 与模型 SDF 里已知的孔几何
直接投影到相机像素坐标，替代 HoughCircles。perception.source 选：
yolo / truth / external —— truth 就走这条路。
为什么不用 Hough：仿真里孔是"方孔 + 贴片亮环"的近似（SDF 没有布尔运算），
靠图像门槛去凑 radus 区间很脆；而孔位本来就是场景里已知量，直接投影
既准又稳，还能当真实检测算法的对照基线。
2. 真值相机（sim_truth_perception.py）：同理，直接算物体位姿，供
visual_servo/filtered_point 用，绕开 ONNX。
3. 补 /home 与 set_named_pose 服务（sim_home_service.py）：真机上这两个
由 Flexiv 驱动提供，仿真里没有 —— 缺了 S01 直接超时。
4. sim_tactile_adapter 容错导入 ros_gz_interfaces：那一路只是"接触点"，
力本身走 geometry_msgs/Wrench。typesupport 加载不了时退化成
contact 恒 False + centroid 恒 0，三力判据（S07 用的）不受影响 ——
宁可用退化模式跑，也不要整个适配器起不来。
5. repo_root 解析（已装包要能找回源码仓库根，见 test_repo_root.py 的用例）
与 use_sim_time 向子 launch 的传递。
另：scripts/gen_sim_handeye.py 把 URDF 真值导出手眼文件（仿真不标定）。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -44,8 +44,13 @@
|
||||
cameras:
|
||||
# --- 腕上相机（eye-in-hand），挂在机械臂法兰上 ------------------------------
|
||||
- gz_ns: /d405 # Gazebo 传感器话题前缀（see arm.sim.sensors.xacro 的 <topic>）
|
||||
ros_ns: d405 # ROS 侧命名空间。应用按 camera_name 参数用这个名字取话题，
|
||||
# 所以它必须等于启动应用时给的 camera_name（默认 d405）
|
||||
# ROS 侧命名空间。应用按 site.yaml → robots[].camera.name 取话题，所以这里
|
||||
# 必须**等于那一项**：仿真那台在 site.yaml 里登记为 robots[] 的 `sn: sim`，
|
||||
# 它的 camera.name 就是这里写的 sim_d405。
|
||||
# 曾经这里写的是 d405，而 site 写的是 d405_left —— 后果是通止规装配在仿真里
|
||||
# S00 报"腕部相机无图像"（图像 13Hz 好好地流着），人只会去怀疑相机。
|
||||
# 改这里的名字，site.yaml 里那个 sn: sim 条目的 camera.name 必须同步改。
|
||||
ros_ns: sim_d405
|
||||
link_frame: sim_d405_link
|
||||
# 图像上盖的坐标系必须是**光学系**，不能是本体系。
|
||||
# gz 与 URDF 里的相机本体遵循"X 朝前 / Y 朝左 / Z 朝上"（相机机身约定），
|
||||
@@ -78,9 +83,14 @@ cameras:
|
||||
# world → d435_link。位姿从 world 文件里**解析**得到（不在这里重复配置），
|
||||
# 避免"改了一处忘了另一处"。
|
||||
- gz_ns: /d435
|
||||
ros_ns: d435
|
||||
link_frame: d435_link
|
||||
optical_frame: d435_color_optical_frame
|
||||
# ROS 侧命名空间 = sim_d435（与真机 d435 分开）。为什么不能沿用 d435：
|
||||
# eye-to-hand 标定文件是按**相机名**命名的（handeye/results/<相机名>_eye_to_hand.yaml），
|
||||
# 仿真自动生成的这份若叫 d435_...，会和真机的手外标定**抢同一个文件名** ——
|
||||
# 后果是"跑一次仿真把真机的标定覆盖了"，而标定文件平时没人看。
|
||||
# frame 名也必须跟着改（默认 <ros_ns>_color_optical_frame，应用按这个查 TF）。
|
||||
ros_ns: sim_d435
|
||||
link_frame: sim_d435_link
|
||||
optical_frame: sim_d435_color_optical_frame
|
||||
depth: true
|
||||
horizontal_fov: 1.5184
|
||||
width: 640
|
||||
|
||||
@@ -50,7 +50,7 @@ table_bounds:
|
||||
# 详见 README 的"仿真 ≠ 真机"一节，那里写了决策点与影响面。
|
||||
operating:
|
||||
# 用哪台相机做孔检测（决定用哪组内参算像素尺度）
|
||||
camera: d405
|
||||
camera: sim_d405
|
||||
# 哪个物体是"工件" —— 相机到工件的距离以**它的上表面**为基准。
|
||||
# 这里写的是物体的 name（不是模型名）。
|
||||
part_object: workpiece
|
||||
|
||||
@@ -94,11 +94,19 @@ sides:
|
||||
# 力传感器所在的坐标系（即 *_inner_finger_joint 的 child link）。
|
||||
# 接触点会转到这个系里，用来算 centroid_u/v，也是轴的参照系。
|
||||
frame: sim_left_finger_mount
|
||||
# 指腹内侧的"夹持点"（= tool center point）。**上料脚本要用它**：
|
||||
# 两指 TCP 的中点就是工具轴，规尖垂在这条轴线上。
|
||||
# 与 frame 的区别：frame 是力传感器的参照系（指尖关节的 child link），
|
||||
# TCP 在它的子链再往下 0.036m（见 flexiv_description 的 finger_tcp_joint）。
|
||||
# 名字不能靠 frame 加后缀推 —— 实测这两条链分别是
|
||||
# sim_left_finger_mount / sim_left_finger_tcp（父链与子链并列命名）。
|
||||
tcp_frame: sim_left_finger_tcp
|
||||
|
||||
- side: right
|
||||
wrench_topic: /tactile/right/wrench
|
||||
contact_topic: /tactile/right/raw
|
||||
frame: sim_right_finger_mount
|
||||
tcp_frame: sim_right_finger_tcp
|
||||
|
||||
# --- 轴向映射（已实测，不是猜的）----------------------------------------
|
||||
# 力传感器挂在指尖关节上，给出的是该关节 child link 坐标系里的三向力。
|
||||
|
||||
@@ -45,7 +45,7 @@
|
||||
d405_y:=-0.06
|
||||
d405_z:=0.05
|
||||
d405_roll:=0.0
|
||||
d405_pitch:=0.0
|
||||
d405_pitch:=-1.5708
|
||||
d405_yaw:=0.0
|
||||
image_width:=848
|
||||
image_height:=480
|
||||
@@ -93,13 +93,32 @@
|
||||
<!-- =======================================================================
|
||||
RealSense D405 —— 眼在手上
|
||||
=======================================================================
|
||||
位姿默认值说明（法兰坐标系，z 朝法兰外侧）：
|
||||
z=+0.05 相机略微伸出法兰，避免看到夹爪本体
|
||||
y=-0.06 偏置到一侧，与真机上的支架安装位置一致
|
||||
位姿默认值说明（法兰坐标系）：
|
||||
xyz: x=0, y=-0.06, z=+0.05
|
||||
z=+0.05 相机略微伸出法兰，避免看到夹爪本体
|
||||
y=-0.06 偏置到一侧，与真机上的支架安装位置一致
|
||||
rpy: roll=0, **pitch=-1.5708(-90°)**, yaw=0
|
||||
这些值**不是**真机的标定结果，只是仿真里的合理起始位。
|
||||
仿真里"手眼关系"本来就精确已知（我们直接定义了相机的安装位姿），
|
||||
所以不需要跑手眼标定 —— 但保留了可调位姿，方便模拟"标定不准"的情况，
|
||||
验证跟踪算法对误差的容忍度。
|
||||
|
||||
⚠️⚠️ pitch 这一项是**必须**的，不是"随手调的角度"。原因在 Gazebo 的
|
||||
相机惯例：**传感器/模型的视轴是 +X**（不是 +Z）。世界相机 d435 就是靠
|
||||
在 world 文件里给它模型 pitch=50° 才瞄准台面的 —— 同一个惯例。
|
||||
腕相机要在法兰垂直朝下时看工件，就得让 d405_link 的 +X 对上法兰的 +Z，
|
||||
即绕 Y 转 -90°。
|
||||
|
||||
为什么"少转这 90°"极难发现（曾经默认 0，踩过）：
|
||||
· 话题有数据、预览有图像、camera_info 内参也对；
|
||||
· 深度反投影甚至"通过"了检查 —— 因为它看到的是**远处的地面**，
|
||||
反投影出来同样是 z≈0 的水平面（check_sim_cameras 当时只看
|
||||
"命中不用平面"，判据太松，现在已改成核对视轴朝向）；
|
||||
· 症状只出现在应用侧：S04 跟踪里真值/检测永远说"目标不在相机前方"，
|
||||
伺服一直"寻找工件"原地转圈 —— 看起来像算法或标定问题。
|
||||
所以：改这里之前先想清楚视轴指向，改完跑
|
||||
python3 scripts/check_sim_cameras.py
|
||||
它会核对"腕部相机光学系的视轴是否与法兰 +Z 一致"。
|
||||
======================================================================= -->
|
||||
<xacro:if value="${d405_enable}">
|
||||
<link name="${prefix}d405_link"/>
|
||||
|
||||
Reference in New Issue
Block a user