arm_dev
6922b18187
为什么
------
应用层只认 arm_msgs/srv/GripperExecute（开口毫米），真机与仿真必须同一套接口。
但 GN01 的 URDF 里 <mimic> 是**线性近似**：直接按线性系数下发 finger_width_joint，
"命令开 20 mm"在 Gazebo 里量出来是 21.75 mm —— 抓不住工件，而且看起来像算法问题。
实测（2026-09-24，Gazebo 内量两指尖间距）：
线性下发 20 mm → 实际 21.74 mm（偏差 1.74 mm，超 2 mm 抓取容差的前半步）
走 FK 反解 → 实际 20.00 mm（≤0.5 mm）
怎么改
------
- 新增 sim_gripper_kin.py：解析 URDF 正运动学，算出"主关节值 ↔ 真实开口 mm"的
单调曲线，反解用二分。曲线非单调/零行程时**主动报错**，不静默退化成线性。
- sim_gripper.py：开口换算全部走该曲线；容差统一改成毫米。
- sim_gripper_adapter.py：反馈改取**从动关节**（见下）；is_gripped 按毫米判定。
- 新增 check_sim_gripper.py：三层验收（服务/开口一致/指尖真实几何行程 + 对称性）。
踩到的坑
--------
- finger_width_joint 挂在无质量的 unused_finger_width_link 上，与手指**没有机械
连接**：命令到 0.100，它的 /joint_states 恒为 1.4e-08，6 个从动关节才准到
0.7854。反馈若取它，/gripper/status 会永远报"开口 0 mm"。
- 残留进程会伪造出"代码写错了"的现场：旧 sim_gripper_adapter 与新实例抢话题，
验收读到"指尖间距 0.03 mm"（真实 127 mm）；孤儿 static_transform_publisher
用 latched /tf_static 把旧相机外参按在新仿真上。
→ 新增 scripts/lib/sim_procs.sh；run_sim_gz.sh 用 setsid 独立进程组，
PGID 记进 log_sim/sim.pgid，stop 时整组 kill；启动前预检拒绝带病启动。
- 验收脚本自己也错过两次（把 TF 的米当毫米比 → 127 mm 显示成 0.13 mm；
用会转动指垫的中点漂移判对称 → 必然位移 16 mm）。判据改成"两侧各走多远"，
单位只在 _dist_mm 里换算一次。
验证
----
python3 -m pytest src/arm_sim_gazebo/test/ -q → 307 passed
python3 scripts/check_sim_gripper.py → 三层全过
第 2 层：请求/反馈开口最大偏差 < 0.5 mm
第 3 层：期望行程 vs 实际行程、左右行程差，均 < 1.5 mm
bash scripts/stop_sim_gz.sh --list → 整组列出（含 static tf）
文档
----
- 仿真手册：新增 §4.4（开口 mm 怎么换算）、坑 12（宽度关节不动、
反馈必须取从动关节）；停止一节改写成进程组语义。
- FRAMEWORK_RULES：新增 R4.11（残留进程会伪造现场）、R4.12（验收判据要挑
与实现无关的物理量）；R4.9 表补夹爪；清单补 13/14 两条。
- HANDBOOK：新增 §3.4「在仿真里开合夹爪」，补 §4.4 的两条新教训。
Co-authored-by: Cursor <cursoragent@cursor.com>
140 lines
6.2 KiB
Bash
Executable File
140 lines
6.2 KiB
Bash
Executable File
#!/usr/bin/env bash
|
||
# =============================================================================
|
||
# 启动 Gazebo 仿真。
|
||
#
|
||
# 用法:
|
||
# bash scripts/run_sim_gz.sh # 无头（无显示器环境用这个）
|
||
# bash scripts/run_sim_gz.sh --gui # 带 Gazebo GUI
|
||
# bash scripts/run_sim_gz.sh --check # 只自检描述，不启动任何东西
|
||
# bash scripts/run_sim_gz.sh --print-urdf # 打印归一化后的 URDF
|
||
# bash scripts/run_sim_gz.sh --dry-run # 只打印将执行的命令
|
||
# bash scripts/run_sim_gz.sh --verbose # 打开 Gazebo 详细日志
|
||
# bash scripts/run_sim_gz.sh --world /abs/other.world
|
||
# bash scripts/run_sim_gz.sh --no-gripper --no-sensors # 最省资源的运动学验证
|
||
#
|
||
# 与 scripts/run_sim.sh 的区别（两个名字容易混，这里说清）：
|
||
# run_sim.sh ← 走 arm_bringup 的 **mock** 驱动：假数据 + RViz，无物理
|
||
# run_sim_gz.sh ← 走 Gazebo：真物理、真接触
|
||
# 两者互不影响，也都不需要真机。要验证力控/接触类逻辑只能用后者。
|
||
#
|
||
# ⚠️ 这个脚本此前**不存在**，却被 install_sim_env.sh（两处）与 build_sim.sh 的
|
||
# 提示语引用 —— 照做会直接报"文件不存在"。现在补上。
|
||
# =============================================================================
|
||
set -euo pipefail
|
||
|
||
ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
|
||
cd "${ROOT}"
|
||
|
||
# 显式映射到 gazebo_sim_launcher 的选项，不做原样透传。
|
||
# 透传会把拼错的选项静默吞掉（ros2 launch 对未知的 key:=value 不报错），
|
||
# 而"我明明设了 --gui 却没生效"这类问题极难查。
|
||
LAUNCH_ARGS=()
|
||
PASS_ONLY=0 # 1 = 只准备描述，不启动（--check / --print-urdf / --dry-run）
|
||
|
||
while [[ $# -gt 0 ]]; do
|
||
case "$1" in
|
||
--gui) LAUNCH_ARGS+=(--headless false); shift ;;
|
||
--headless) LAUNCH_ARGS+=(--headless true); shift ;;
|
||
--verbose|-v) LAUNCH_ARGS+=(--verbose); shift ;;
|
||
--no-gripper) LAUNCH_ARGS+=(--no-gripper); shift ;;
|
||
--no-sensors) LAUNCH_ARGS+=(--no-sensors); shift ;;
|
||
--check|--print-urdf|--dry-run)
|
||
LAUNCH_ARGS+=("$1"); PASS_ONLY=1; shift ;;
|
||
--world)
|
||
[[ -n "${2:-}" ]] || { echo "--world 需要跟一个路径" >&2; exit 2; }
|
||
LAUNCH_ARGS+=(--world "$2"); shift 2 ;;
|
||
-h|--help) sed -n '2,18p' "$0"; exit 0 ;;
|
||
*) echo "未知参数: $1（用 --help 看用法）" >&2; exit 2 ;;
|
||
esac
|
||
done
|
||
|
||
if [[ ! -f "${ROOT}/install_sim/setup.bash" ]]; then
|
||
echo "install_sim/ 还不存在。先跑:" >&2
|
||
echo " bash scripts/build_sim.sh" >&2
|
||
exit 1
|
||
fi
|
||
|
||
# sim_env.sh 会先退到 base 再激活目标环境，避免同一 shell 里来回切造成的
|
||
# PATH/PYTHONPATH 残留（详见 scripts/lib/conda_switch.sh 顶部的实测说明）。
|
||
# shellcheck disable=SC1091
|
||
source "${ROOT}/scripts/sim_env.sh"
|
||
|
||
if ! command -v ign >/dev/null 2>&1; then
|
||
echo "找不到 ign（Gazebo Fortress 的 CLI）。先跑:" >&2
|
||
echo " bash scripts/install_sim_env.sh" >&2
|
||
exit 1
|
||
fi
|
||
|
||
if [[ "${PASS_ONLY}" -eq 1 ]]; then
|
||
# 这些选项不启动任何东西，只准备描述，直接转发。
|
||
# （所以放在"残留进程"预检**之前**：手上有仿真在跑时，照样应该能 --print-urdf。）
|
||
exec ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}"
|
||
fi
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 预检：不许在"上一轮仿真还有残留"的情况下启动
|
||
# ---------------------------------------------------------------------------
|
||
# ⚠️ 这不是洁癖。实测过的最坏情况：残留了一个 sim_gripper_adapter，新起一个之后
|
||
# 两个适配器**同时**订阅 /joint_states、同时发布 /gripper/status，
|
||
# 服务调用还会被随机分给其中一个 —— 验收脚本读到的开口恒为 0 mm、
|
||
# 指尖间距 0.03 mm（真实值 127 mm），看起来完全像夹爪换算写错了。
|
||
# 残留的 static_transform_publisher 更阴：它发 latched 的 /tf_static，
|
||
# 会一直把**旧外参**按在新仿真的 TF 树上。
|
||
# 所以这里宁可拒绝启动，也不给你一个"看起来能跑、数据是错的"仿真。
|
||
# shellcheck source=lib/sim_procs.sh
|
||
source "${ROOT}/scripts/lib/sim_procs.sh"
|
||
|
||
mapfile -t running < <(sim_proc_targets)
|
||
if [[ "${#running[@]}" != "0" ]]; then
|
||
{
|
||
echo "已有仿真相关进程在跑，拒绝启动第二个："
|
||
for pid in "${running[@]}"; do
|
||
printf ' %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-120)"
|
||
done
|
||
echo
|
||
echo "先停掉它们： bash scripts/stop_sim_gz.sh"
|
||
echo "只想看看残留是谁、不动手：bash scripts/stop_sim_gz.sh --list"
|
||
} >&2
|
||
exit 3
|
||
fi
|
||
|
||
echo "=== 启动 Gazebo 仿真 ==="
|
||
echo " 环境 : ${CONDA_DEFAULT_ENV:-?} ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-?}"
|
||
echo " 退出方式 : Ctrl-C"
|
||
echo
|
||
echo " 首次启动约需 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。"
|
||
echo " 验收: ros2 control list_controllers 应看到两个 active"
|
||
echo
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 起在自己的**会话/进程组**里，并把 PGID 记下来
|
||
# ---------------------------------------------------------------------------
|
||
# 这样 stop_sim_gz.sh 可以用 `kill -TERM -PGID` 一次性带走整栈（含
|
||
# robot_state_publisher / static_transform_publisher 这些"不在 install_sim 下"
|
||
# 的节点），不必依赖模式匹配 —— 模式表漏一个就留一个孤儿的教训见脚本注释。
|
||
#
|
||
# 代价：进程组不在前台，终端里的 Ctrl-C **不会**传到仿真。所以这里自己接
|
||
# SIGINT/SIGTERM，转发成组信号，行为与以前"Ctrl-C 停仿真"一致。
|
||
PGID_FILE="$(sim_pgid_file)"
|
||
mkdir -p "$(dirname "${PGID_FILE}")"
|
||
|
||
setsid bash -c 'echo $$ > "$1"; shift; exec "$@"' _ "${PGID_FILE}" \
|
||
ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}" &
|
||
SIM_PID=$!
|
||
|
||
_forward_signal() {
|
||
local pgid
|
||
pgid="$(sim_pgid_read)"
|
||
if [[ -n "${pgid}" ]]; then
|
||
echo
|
||
echo "收到中断信号，正在停止整栈（进程组 ${pgid}）…" >&2
|
||
sim_pgid_stop "${pgid}"
|
||
fi
|
||
}
|
||
trap _forward_signal INT TERM
|
||
|
||
wait "${SIM_PID}"
|
||
status=$?
|
||
rm -f "${PGID_FILE}"
|
||
exit "${status}"
|