左臂换成 tool10120938 后，手眼仍按旧 tool1 和 8×11 板求解，顶视迁臂又按短跳 rest-to-rest，真机上要么整体偏一截，要么中途弹射启停。
眼在手上改用该 TCP 做 AX=XB，再折回相机→法兰；A5 内角点改为 11×8，09-04 / 10-09 旧结果留在 backup。一阶段 D435 迁臂改成一条 FollowJointTrajectory（余弦加减速，近处降速，XY 进门槛后再单独收垂直），跟踪锁高与悬停齐平，避免倾斜相机下先降一截把孔位重投影偏掉。--no-descent 只降到离面 15 mm；跳过三阶段时触觉 USB 打不开不再空等几分钟。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -42,7 +42,29 @@ pick_iface() {
|
||||
echo "${SITE_NET_IFACE}"
|
||||
return
|
||||
fi
|
||||
for d in enp4s0 enp5s0 eth0; do
|
||||
# 内核有时把 USB 网卡叫 eth0，不再用 enx+MAC。按 MAC 找回，禁止误选办公网 enp4s0。
|
||||
local want="${SITE_NET_IFACE:-}"
|
||||
if [[ "${want}" == enx* && ${#want} -ge 15 ]]; then
|
||||
local mac
|
||||
mac="$(printf '%s' "${want#enx}" | sed 's/../&:/g;s/:$//')"
|
||||
local d
|
||||
for d in /sys/class/net/*; do
|
||||
[[ -f "${d}/address" ]] || continue
|
||||
if [[ "$(cat "${d}/address")" == "${mac}" ]]; then
|
||||
echo "$(basename "$d")"
|
||||
return
|
||||
fi
|
||||
done
|
||||
fi
|
||||
local d
|
||||
for d in /sys/class/net/*; do
|
||||
[[ -f "${d}/address" ]] || continue
|
||||
if ip -4 -o addr show "$(basename "$d")" 2>/dev/null | grep -q '192.168.2.10'; then
|
||||
echo "$(basename "$d")"
|
||||
return
|
||||
fi
|
||||
done
|
||||
for d in eth0 enp4s0 enp5s0; do
|
||||
[[ -d /sys/class/net/$d ]] || continue
|
||||
echo "$d"
|
||||
return
|
||||
|
||||
@@ -627,6 +627,13 @@ def flatten(data: dict) -> dict:
|
||||
# 两个模式各自的名字在 *_EYE_IN_HAND / *_EYE_TO_HAND 里。
|
||||
"SITE_HANDEYE_FILE": he_modes_out["SITE_HANDEYE_FILE_EYE_IN_HAND"],
|
||||
"SITE_HANDEYE_MODE": he_mode,
|
||||
"SITE_HANDEYE_GRIPPER_TOOL": str(he.get("gripper_tool") or ""),
|
||||
"SITE_HANDEYE_GRIPPER_TCP_XYZ": ",".join(
|
||||
f"{float(v):.6f}" for v in list(he.get("gripper_tcp_xyz") or [])
|
||||
),
|
||||
"SITE_HANDEYE_GRIPPER_TCP_QWXYZ": ",".join(
|
||||
f"{float(v):.6f}" for v in list(he.get("gripper_tcp_qwxyz") or [])
|
||||
),
|
||||
**he_modes_out,
|
||||
# 不带后缀的板尺寸 = **当前模式**那一组，供 `show` 之类只看当前模式的调用方；
|
||||
# 标定脚本用带后缀的那三组（按 --mode 选），不依赖这个。
|
||||
@@ -985,6 +992,10 @@ def main() -> int:
|
||||
print(f" 手眼 : {flat['SITE_HANDEYE_FILE']}")
|
||||
print(f" 手眼路点 : {flat['SITE_HANDEYE_WAYPOINTS']}")
|
||||
print(f" 手眼目录 : {flat['SITE_HANDEYE_WORKDIR']}")
|
||||
print(
|
||||
f" 手眼工具 : {flat.get('SITE_HANDEYE_GRIPPER_TOOL') or '（无，ee_pose 原样）'} "
|
||||
f"tcp=[{flat.get('SITE_HANDEYE_GRIPPER_TCP_XYZ') or ''}]"
|
||||
)
|
||||
print(f" 命名位姿 : {flat.get('SITE_NAMED_POSES_FILE')}")
|
||||
print(f" 眼在手外结果 : {flat['SITE_HANDEYE_FILE_EYE_TO_HAND']}")
|
||||
print(f" 眼在手外路点 : {flat['SITE_HANDEYE_WAYPOINTS_EYE_TO_HAND']}")
|
||||
|
||||
@@ -23,6 +23,7 @@ RDK 对同一台机器人是**独占访问**：同时有两个进程连着同一
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import os
|
||||
import time
|
||||
from typing import Any, Dict, List, Optional
|
||||
|
||||
@@ -155,6 +156,77 @@ def tool_params(tool: Any, group: Any) -> Dict[str, Any]:
|
||||
}
|
||||
|
||||
|
||||
def iface_ipv4s(name: str) -> List[str]:
|
||||
"""网卡名 → 其上的 IPv4。FastDDS 白名单只认地址，不认 ``eth0`` / ``enx…``。"""
|
||||
import fcntl
|
||||
import socket
|
||||
import struct
|
||||
import subprocess
|
||||
|
||||
name = (name or "").strip()
|
||||
if not name:
|
||||
return []
|
||||
if name.count(".") == 3:
|
||||
return [name]
|
||||
addrs: List[str] = []
|
||||
try:
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
ifreq = struct.pack("256s", name.encode("utf-8")[:15])
|
||||
addrs.append(socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, ifreq)[20:24]))
|
||||
s.close()
|
||||
except OSError:
|
||||
pass
|
||||
try:
|
||||
out = subprocess.check_output(
|
||||
["ip", "-4", "-o", "addr", "show", "dev", name],
|
||||
text=True,
|
||||
stderr=subprocess.DEVNULL,
|
||||
)
|
||||
for line in out.splitlines():
|
||||
parts = line.split()
|
||||
if "inet" in parts:
|
||||
addrs.append(parts[parts.index("inet") + 1].split("/")[0])
|
||||
except Exception:
|
||||
pass
|
||||
seen = set()
|
||||
uniq: List[str] = []
|
||||
for a in addrs:
|
||||
if a and a not in seen:
|
||||
seen.add(a)
|
||||
uniq.append(a)
|
||||
return uniq
|
||||
|
||||
|
||||
def rdk_whitelist_lists(iface: str = "", pc_ip: str = "") -> List[List[str]]:
|
||||
"""给 ``flexivrdk.Robot(sn, listen)`` 用的白名单候选。
|
||||
|
||||
只放 IPv4。把 ``eth0`` 这种网卡名塞进去，FastDDS 会把真地址全部滤掉，
|
||||
症状是 ping 通、RDK 报 Failed to connect。
|
||||
"""
|
||||
lists: List[List[str]] = []
|
||||
pc = (pc_ip or os.environ.get("SITE_PC_IP") or "").strip()
|
||||
if pc.count(".") == 3:
|
||||
lists.append([pc])
|
||||
iface = (iface or "").strip()
|
||||
if iface.count(".") == 3:
|
||||
lists.append([iface])
|
||||
elif iface:
|
||||
lan = [a for a in iface_ipv4s(iface) if a.startswith("192.168.2.")]
|
||||
if lan:
|
||||
lists.append(lan)
|
||||
all_ips = iface_ipv4s(iface)
|
||||
if all_ips and all_ips != lan:
|
||||
lists.append(all_ips)
|
||||
seen = set()
|
||||
out: List[List[str]] = []
|
||||
for item in lists:
|
||||
key = tuple(item)
|
||||
if key not in seen:
|
||||
seen.add(key)
|
||||
out.append(item)
|
||||
return out
|
||||
|
||||
|
||||
def switch_tool(tool: Any, group: Any, target: str) -> None:
|
||||
"""切换工具。失败抛异常。"""
|
||||
if group is None:
|
||||
|
||||
@@ -61,6 +61,16 @@ def parse_poses(path: Path) -> list[list[float]]:
|
||||
return poses
|
||||
|
||||
|
||||
def unique_poses(poses: list[list[float]], tol: float = 0.015) -> list[list[float]]:
|
||||
"""关节几乎不动的连拍只留一条。阈值 0.015 rad ≈ 0.86°。"""
|
||||
kept: list[list[float]] = []
|
||||
for pose in poses:
|
||||
if any(max(abs(a - b) for a, b in zip(pose, old)) < tol for old in kept):
|
||||
continue
|
||||
kept.append(pose)
|
||||
return kept
|
||||
|
||||
|
||||
def format_positions_block(poses: list[list[float]], indent: str = " ") -> str:
|
||||
rows = [f"{indent}waypoint_positions: ["]
|
||||
for i, q in enumerate(poses):
|
||||
@@ -131,9 +141,19 @@ def main(argv: list[str]) -> int:
|
||||
print(f"Not found: {dst}", file=sys.stderr)
|
||||
return 3
|
||||
poses = parse_poses(src)
|
||||
if len(poses) < 5:
|
||||
print(f"Need >=5 poses, got {len(poses)}", file=sys.stderr)
|
||||
uniq = unique_poses(poses)
|
||||
if len(uniq) < 5:
|
||||
print(
|
||||
f"Need >=5 unique poses, got {len(poses)} lines / {len(uniq)} unique",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 4
|
||||
if len(uniq) < len(poses):
|
||||
print(
|
||||
f"警告: {len(poses)} 条里只有 {len(uniq)} 个姿态不同，已丢掉连按重复",
|
||||
file=sys.stderr,
|
||||
)
|
||||
poses = uniq
|
||||
merge(dst, poses)
|
||||
print(f"Merged {len(poses)} poses -> {dst}")
|
||||
return 0
|
||||
|
||||
@@ -55,6 +55,7 @@ from rdk_helpers import ( # noqa: E402
|
||||
clear_fault as rdk_clear_fault,
|
||||
ensure_idle,
|
||||
ensure_operational,
|
||||
rdk_whitelist_lists,
|
||||
resolve_tool_name,
|
||||
single_arm_groups,
|
||||
switch_tool,
|
||||
@@ -228,8 +229,11 @@ def main() -> int:
|
||||
robot = None
|
||||
last_err = None
|
||||
attempts = []
|
||||
if iface:
|
||||
attempts.extend([(sn, [iface]), (sn, [iface], True)])
|
||||
# 白名单必须是 IPv4。传 eth0 / enx… 会被 FastDDS 整段滤掉，
|
||||
# ping 通也会 Failed to connect。
|
||||
for listen in rdk_whitelist_lists(iface):
|
||||
log('白名单: ' + ', '.join(listen))
|
||||
attempts.extend([(sn, listen), (sn, listen, True)])
|
||||
attempts.append((sn,))
|
||||
for args in attempts:
|
||||
try:
|
||||
|
||||
@@ -0,0 +1,204 @@
|
||||
#!/usr/bin/env python3
|
||||
"""用厂家 Flexiv RDK 的原语 MoveL，把当前工具尖走到世界系里的一个点。
|
||||
|
||||
必须用 RDK 自带的 Python 3.10，不能用 ROS 的 3.11：
|
||||
|
||||
# 先在跑 run_real.sh 的那个终端 Ctrl-C。RDK 同时只能有一个连接。
|
||||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_goto_xyz.py
|
||||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_goto_xyz.py --xyz 0.45 -0.30 0.40
|
||||
|
||||
不给 --xyz 时只打印当前工具尖坐标，不动臂。
|
||||
|
||||
这个脚本做的事，按顺序只有这些：
|
||||
|
||||
1. 用网卡上的 IPv4 连上序列号那台臂。网卡名（eth0）不能当白名单。
|
||||
2. 清故障、使能，等到 operational。示教器要在 AUTO Remote，急停松开。
|
||||
3. 读 states().tcp_pose。这 7 个数是
|
||||
[x, y, z, qw, qx, qy, qz]，单位米，坐标系是机器人世界系（基座那套 WORLD）。
|
||||
位置是当前 Studio 工具的 TCP（现在是 tool10120938 的尖），不是法兰，也不是相机。
|
||||
4. 目标只换 x、y、z。姿态沿用现在的朝向，折成厂家 MoveL 要的欧拉角（度，ZYX）。
|
||||
5. SwitchMode(NRT_PRIMITIVE_EXECUTION)，ExecutePrimitive("MoveL", ...)。
|
||||
6. 轮询 primitive_states()["reachedTarget"]。到了再读一次 tcp_pose，印出还差多少毫米。
|
||||
|
||||
厂家示例：flexiv_rdk example_py/basics3_primitive_execution.py 第 (3) 段。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
sys.path.insert(0, str(ROOT / "scripts" / "lib"))
|
||||
|
||||
from rdk_helpers import rdk_whitelist_lists # noqa: E402
|
||||
|
||||
|
||||
def site_robot() -> tuple[str, str]:
|
||||
"""读 config/site.yaml 里第一台臂的序列号和这台电脑的机器人网 IP。"""
|
||||
text = (ROOT / "config" / "site.yaml").read_text(encoding="utf-8")
|
||||
sn = ""
|
||||
pc_ip = ""
|
||||
for raw in text.splitlines():
|
||||
line = raw.split("#", 1)[0].strip()
|
||||
if line.startswith("sn:") and not sn:
|
||||
sn = line.split(":", 1)[1].strip().strip('"').strip("'")
|
||||
elif line.startswith("pc_ip:") and not pc_ip:
|
||||
pc_ip = line.split(":", 1)[1].strip().strip('"').strip("'")
|
||||
if sn and pc_ip:
|
||||
break
|
||||
if not sn:
|
||||
raise RuntimeError(f"config/site.yaml 里没有 sn：{ROOT / 'config' / 'site.yaml'}")
|
||||
return sn, pc_ip
|
||||
|
||||
|
||||
def quat_wxyz_to_euler_xyz_deg(q) -> list[float]:
|
||||
"""[qw,qx,qy,qz] → MoveL 要的 [rot_x, rot_y, rot_z] 度。
|
||||
|
||||
与厂家 example_py/utility.py 的 quat2eulerZYX 相同：
|
||||
scipy 的 from_quat([x,y,z,w]).as_euler('xyz', degrees=True)。
|
||||
这里不 import scipy，避免和 ROS 环境的 scipy 打架。
|
||||
"""
|
||||
qw, qx, qy, qz = (float(v) for v in q)
|
||||
n = math.sqrt(qw * qw + qx * qx + qy * qy + qz * qz)
|
||||
if n < 1e-12:
|
||||
raise RuntimeError(f"四元数是零：{q}")
|
||||
qw, qx, qy, qz = qw / n, qx / n, qy / n, qz / n
|
||||
xx, yy, zz = qx * qx, qy * qy, qz * qz
|
||||
xy, xz, yz = qx * qy, qx * qz, qy * qz
|
||||
wx, wy, wz = qw * qx, qw * qy, qw * qz
|
||||
r00 = 1.0 - 2.0 * (yy + zz)
|
||||
r10 = 2.0 * (xy + wz)
|
||||
r20 = 2.0 * (xz - wy)
|
||||
r21 = 2.0 * (yz + wx)
|
||||
r22 = 1.0 - 2.0 * (xx + yy)
|
||||
r01 = 2.0 * (xy - wz)
|
||||
r11 = 1.0 - 2.0 * (xx + zz)
|
||||
r12 = 2.0 * (yz - wx)
|
||||
# R = Rx(x) @ Ry(y) @ Rz(z)
|
||||
sy = math.sqrt(r00 * r00 + r10 * r10)
|
||||
if sy > 1e-6:
|
||||
x = math.atan2(r21, r22)
|
||||
y = math.atan2(-r20, sy)
|
||||
z = math.atan2(r10, r00)
|
||||
else:
|
||||
x = math.atan2(-r12, r11)
|
||||
y = math.atan2(-r20, sy)
|
||||
z = 0.0
|
||||
return [math.degrees(x), math.degrees(y), math.degrees(z)]
|
||||
|
||||
|
||||
def connect(flexivrdk, sn: str, pc_ip: str):
|
||||
"""白名单只放 IPv4。连不上就换下一组，最后再试不带白名单。"""
|
||||
last = None
|
||||
attempts = []
|
||||
for listen in rdk_whitelist_lists(pc_ip=pc_ip):
|
||||
attempts.append((sn, listen))
|
||||
attempts.append((sn,))
|
||||
for args in attempts:
|
||||
try:
|
||||
print(f"连接 {args}")
|
||||
return flexivrdk.Robot(*args)
|
||||
except Exception as exc: # noqa: BLE001 — 下一组白名单再试，最后一组失败才抛
|
||||
last = exc
|
||||
print(f" 失败：{exc}")
|
||||
raise RuntimeError(f"RDK 连不上 {sn}：{last}")
|
||||
|
||||
|
||||
def wait_operational(robot, timeout_s: float) -> None:
|
||||
if robot.fault():
|
||||
print("有故障，尝试 ClearFault")
|
||||
if not robot.ClearFault():
|
||||
raise RuntimeError("故障清不掉。看示教器。")
|
||||
print("Enable …（示教器：松开急停，AUTO Remote）")
|
||||
robot.Enable()
|
||||
t0 = time.monotonic()
|
||||
while not robot.operational():
|
||||
if time.monotonic() - t0 > timeout_s:
|
||||
raise RuntimeError(f"{timeout_s:.0f}s 内没有 operational")
|
||||
time.sleep(0.2)
|
||||
print("operational")
|
||||
|
||||
|
||||
def fmt_pose(pose) -> str:
|
||||
x, y, z = pose[0], pose[1], pose[2]
|
||||
return f"xyz=({x:.4f}, {y:.4f}, {z:.4f}) m qwxyz=({pose[3]:.4f}, {pose[4]:.4f}, {pose[5]:.4f}, {pose[6]:.4f})"
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description="RDK MoveL：工具尖走到世界系的一个点")
|
||||
parser.add_argument("--xyz", nargs=3, type=float, metavar=("X", "Y", "Z"), help="目标点，米，机器人世界系")
|
||||
parser.add_argument("--vel", type=float, default=0.05, help="TCP 线速度 m/s，默认 0.05")
|
||||
parser.add_argument("--max-step-m", type=float, default=0.40, help="离当前点超过这么多米就拒绝，除非 --go")
|
||||
parser.add_argument("--go", action="store_true", help="允许超过 --max-step-m")
|
||||
parser.add_argument("--timeout", type=float, default=60.0, help="等 reachedTarget 的秒数")
|
||||
args = parser.parse_args()
|
||||
|
||||
sn, pc_ip = site_robot()
|
||||
print(f"臂 {sn} 本机 {pc_ip or '（site 里没有 pc_ip）'}")
|
||||
|
||||
import flexivrdk
|
||||
|
||||
robot = connect(flexivrdk, sn, pc_ip)
|
||||
try:
|
||||
wait_operational(robot, args.timeout)
|
||||
pose = list(robot.states().tcp_pose)
|
||||
if len(pose) < 7:
|
||||
raise RuntimeError(f"tcp_pose 不是 7 个数：{pose}")
|
||||
print("当前 TCP（世界系）：", fmt_pose(pose))
|
||||
if args.xyz is None:
|
||||
print("没有 --xyz，只看坐标，不动。")
|
||||
return 0
|
||||
|
||||
target_xyz = [float(v) for v in args.xyz]
|
||||
dist = math.dist(pose[:3], target_xyz)
|
||||
print(f"目标 xyz=({target_xyz[0]:.4f}, {target_xyz[1]:.4f}, {target_xyz[2]:.4f}) m")
|
||||
print(f"直线距离 {dist * 1000:.0f} mm 速度 {args.vel:.3f} m/s 姿态保持当前")
|
||||
if dist > args.max_step_m and not args.go:
|
||||
print(f"超过 {args.max_step_m * 1000:.0f} mm，拒绝。确认无误后加 --go。")
|
||||
return 2
|
||||
|
||||
euler_deg = quat_wxyz_to_euler_xyz_deg(pose[3:7])
|
||||
print(f"MoveL 欧拉角 deg xyz=({euler_deg[0]:.2f}, {euler_deg[1]:.2f}, {euler_deg[2]:.2f})")
|
||||
|
||||
robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
|
||||
robot.ExecutePrimitive(
|
||||
"MoveL",
|
||||
{
|
||||
"target": flexivrdk.Coord(
|
||||
target_xyz,
|
||||
euler_deg,
|
||||
["WORLD", "WORLD_ORIGIN"],
|
||||
),
|
||||
"vel": float(args.vel),
|
||||
},
|
||||
)
|
||||
print("MoveL 已下发，等 reachedTarget …")
|
||||
t0 = time.monotonic()
|
||||
while True:
|
||||
states = robot.primitive_states()
|
||||
if states.get("reachedTarget"):
|
||||
break
|
||||
if robot.fault():
|
||||
raise RuntimeError(f"运动中故障：{states}")
|
||||
if time.monotonic() - t0 > args.timeout:
|
||||
raise RuntimeError(f"{args.timeout:.0f}s 内没有 reachedTarget。states={states}")
|
||||
time.sleep(0.2)
|
||||
|
||||
end = list(robot.states().tcp_pose)
|
||||
err_mm = math.dist(end[:3], target_xyz) * 1000.0
|
||||
print("到达 TCP：", fmt_pose(end))
|
||||
print(f"相对目标还差 {err_mm:.1f} mm")
|
||||
return 0
|
||||
finally:
|
||||
try:
|
||||
robot.Stop()
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -158,27 +158,11 @@ def _iface_ipv4s(name: str) -> list:
|
||||
|
||||
|
||||
def _rdk_whitelist(iface: str) -> list[list[str]]:
|
||||
"""RDK 手册要的是「连机器人的那张网卡」。enp4s0 上还有办公网 IP，不能整张卡的地址都塞进白名单。"""
|
||||
import os
|
||||
"""只放 IPv4。网卡名进白名单会被 FastDDS 滤掉（见 rdk_helpers）。"""
|
||||
sys.path.insert(0, str(__import__("pathlib").Path(__file__).resolve().parent / "lib"))
|
||||
from rdk_helpers import rdk_whitelist_lists # noqa: PLC0415
|
||||
|
||||
lists: list[list[str]] = []
|
||||
pc = (os.environ.get("SITE_PC_IP") or "").strip()
|
||||
if pc:
|
||||
lists.append([pc])
|
||||
if iface:
|
||||
lists.append([iface])
|
||||
robot_lan = [a for a in _iface_ipv4s(iface) if a.startswith("192.168.2.")]
|
||||
if robot_lan:
|
||||
lists.append(robot_lan)
|
||||
# 去重
|
||||
seen: set[tuple[str, ...]] = set()
|
||||
out: list[list[str]] = []
|
||||
for item in lists:
|
||||
key = tuple(item)
|
||||
if key not in seen:
|
||||
seen.add(key)
|
||||
out.append(item)
|
||||
return out
|
||||
return rdk_whitelist_lists(iface)
|
||||
|
||||
|
||||
def connect_rdk(sn: str, iface: str = ""):
|
||||
|
||||
+29
-9
@@ -20,7 +20,8 @@
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview # 相机带 Qt 预览窗
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera # 相机已在别处拉起了
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile # 不起触觉服务
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 只对准不轻降（验证精对准）
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 对准后只降到观测高度（默认离面 15mm）
|
||||
# bash ~/ros2/scripts/run_all.sh --arm 89 --d435-only # 只测 D435 单次位移，到位后不交 D405、不进二阶段
|
||||
#
|
||||
# 前置（另开一个终端，先跑起来）：
|
||||
# bash ~/ros2/scripts/run_real.sh --arm 89
|
||||
@@ -43,6 +44,7 @@ WITH_CAMERA=true
|
||||
WITH_TACTILE=true
|
||||
CAMERA_PREVIEW=false
|
||||
NO_DESCENT=false
|
||||
D435_ONLY=false
|
||||
|
||||
# --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
|
||||
# shellcheck disable=SC1091
|
||||
@@ -59,14 +61,15 @@ while [[ $# -gt 0 ]]; do
|
||||
--depth) DEPTH="${2:?--depth 需要毫米数}"; shift 2 ;;
|
||||
--pitch) PITCH="${2:?--pitch 需要螺距毫米数}"; shift 2 ;;
|
||||
--resnap) RESNAP=true; shift ;;
|
||||
# 只验证**精对准**、不轻降。对准逻辑改完先这么跑，确认规真被对到孔心再放开下降
|
||||
# （对不准就下压会把规顶在孔沿上：2026-10-08 真机顶到 63N）。
|
||||
# 不到贴面 2mm。对准后仍降到 site 的 observe_gap_mm（默认离面 15mm），
|
||||
# 方便看规尖偏没偏；对不准就下到 2mm 会把规顶在孔沿上（2026-10-08 真机 63N）。
|
||||
--no-descent) NO_DESCENT=true; shift ;;
|
||||
--d435-only) D435_ONLY=true; SKIP_STAGE2=true; SKIP_STAGE3=true; shift ;;
|
||||
--no-preview) PIPELINE_PREVIEW=false; shift ;;
|
||||
--camera-preview) CAMERA_PREVIEW=true; shift ;;
|
||||
--no-camera) WITH_CAMERA=false; shift ;;
|
||||
--no-tactile) WITH_TACTILE=false; shift ;;
|
||||
-h|--help) sed -n '2,30p' "$0"; exit 0 ;;
|
||||
-h|--help) sed -n '2,31p' "$0"; exit 0 ;;
|
||||
*) echo "Unknown: $1" >&2; exit 1 ;;
|
||||
esac
|
||||
done
|
||||
@@ -174,12 +177,21 @@ else
|
||||
echo "=== --no-camera：跳过相机检查 ==="
|
||||
fi
|
||||
|
||||
# ---- 3) 触觉服务。只有左臂装了传感器 ----
|
||||
# ---- 3) 触觉服务。只有左臂装了传感器；只有三阶段搜孔才用力 ----
|
||||
# --skip-stage3 时如果还去等，USB 打不开会假死：话题在、connected 却是 false，
|
||||
# 每次 echo 吃满超时 × 60 次 ≈ 3 分钟，终端上没有任何新行（2026-10-09 真机）。
|
||||
_SKIP_TACTILE_REASON=""
|
||||
if [[ "${SKIP_STAGE3}" == true && "${WITH_TACTILE}" == true ]]; then
|
||||
echo "=== --skip-stage3：一/二阶段不用触觉，跳过拉起 ==="
|
||||
WITH_TACTILE=false
|
||||
_SKIP_TACTILE_REASON="stage3"
|
||||
fi
|
||||
|
||||
# 同样先查 topic 列表再 echo，避免服务没起时反复撞超时。
|
||||
TACTILE_TOPIC="/tactile/left/status"
|
||||
_tactile_ok() {
|
||||
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${TACTILE_TOPIC}" || return 1
|
||||
timeout 3 ros2 topic echo "${TACTILE_TOPIC}" --once 2>/dev/null | grep -q "connected: true"
|
||||
timeout 1 ros2 topic echo "${TACTILE_TOPIC}" --once 2>/dev/null | grep -q "connected: true"
|
||||
}
|
||||
|
||||
if [[ "${WITH_TACTILE}" == true ]]; then
|
||||
@@ -199,12 +211,19 @@ if [[ "${WITH_TACTILE}" == true ]]; then
|
||||
sleep 1.0
|
||||
nohup ros2 launch arm_application tactile.launch.py > /tmp/run_all_tactile.log 2>&1 &
|
||||
echo " tactile pid=$! log=/tmp/run_all_tactile.log"
|
||||
echo " 等触觉 connected（USB 打不开会在十几秒内跳过，不会空等几分钟）"
|
||||
t_ok=false
|
||||
for _i in $(seq 1 60); do # 最多等 ~30s；传感器要在 Python 侧加载模型
|
||||
for _i in $(seq 1 8); do
|
||||
if _tactile_ok; then
|
||||
t_ok=true
|
||||
break
|
||||
fi
|
||||
# 日志已经写明打不开：再给两轮自愈，然后放行。不许在这里空转到超时。
|
||||
if [[ "${_i}" -ge 4 ]] && grep -q "connect failed" /tmp/run_all_tactile.log 2>/dev/null; then
|
||||
echo " 触觉 USB 打不开，停止等待" >&2
|
||||
break
|
||||
fi
|
||||
echo " 触觉还未 connected（${_i}/8）"
|
||||
sleep 0.5
|
||||
done
|
||||
if [[ "${t_ok}" != true ]]; then
|
||||
@@ -219,7 +238,7 @@ if [[ "${WITH_TACTILE}" == true ]]; then
|
||||
else
|
||||
echo "=== 触觉只有左臂装了；当前 ${SITE_ROBOT_ARM:-?}，跳过 ==="
|
||||
fi
|
||||
else
|
||||
elif [[ "${_SKIP_TACTILE_REASON}" != "stage3" ]]; then
|
||||
echo "=== --no-tactile：跳过触觉检查 ==="
|
||||
fi
|
||||
|
||||
@@ -239,10 +258,11 @@ ARGS=(
|
||||
"preview:=${PIPELINE_PREVIEW}"
|
||||
"resnap:=${RESNAP}"
|
||||
"no_descent:=${NO_DESCENT}"
|
||||
"d435_move_only:=${D435_ONLY}"
|
||||
)
|
||||
[[ -n "${HOLE}" ]] && ARGS+=("target_hole:=${HOLE}")
|
||||
[[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}")
|
||||
[[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}")
|
||||
|
||||
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} ==="
|
||||
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} d435_only=${D435_ONLY} ==="
|
||||
exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}"
|
||||
|
||||
+31
-1
@@ -202,6 +202,7 @@ cmd_show() {
|
||||
echo " result : ${RES}"
|
||||
echo " workdir : ${WORKDIR}"
|
||||
echo " camera : ${CAM} serial=${CAM_SERIAL:-（空）}"
|
||||
echo " tool : ${SITE_ROBOT_TOOL_NAME:-（不切换）} gripper_tool=${SITE_HANDEYE_GRIPPER_TOOL:-（无，ee_pose 原样）}"
|
||||
echo " board : $(_he BOARD_COLS)x$(_he BOARD_ROWS) inner, square=$(_he SQUARE) m (来源=$(_he BOARD_SRC))"
|
||||
echo " （另一种模式的板见下面 export 的 标定板[...] 两行）"
|
||||
python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
|
||||
@@ -240,6 +241,20 @@ cmd_calibrate() {
|
||||
exit 2
|
||||
fi
|
||||
_need_files "${CAL}" "${WP}"
|
||||
local grip_tool="${SITE_HANDEYE_GRIPPER_TOOL:-}"
|
||||
local grip_xyz="${SITE_HANDEYE_GRIPPER_TCP_XYZ:-}"
|
||||
local grip_q="${SITE_HANDEYE_GRIPPER_TCP_QWXYZ:-}"
|
||||
if [[ -n "${grip_tool}" ]]; then
|
||||
if [[ "${SITE_ROBOT_TOOL_NAME:-}" != "${grip_tool}" ]]; then
|
||||
echo "ERROR: 手眼要按工具 ${grip_tool} 求解，但 site 当前激活工具是 '${SITE_ROBOT_TOOL_NAME:-}'。" >&2
|
||||
echo " robots[].tool_name 必须和 handeye.gripper_tool 同一份名字。" >&2
|
||||
exit 2
|
||||
fi
|
||||
if [[ -z "${grip_xyz}" || -z "${grip_q}" ]]; then
|
||||
echo "ERROR: 配了 gripper_tool=${grip_tool} 但没写 gripper_tcp_xyz / gripper_tcp_qwxyz。" >&2
|
||||
exit 2
|
||||
fi
|
||||
fi
|
||||
_need_robot
|
||||
_need_camera
|
||||
echo "=== kill visual_servo (avoid traj fight) ==="
|
||||
@@ -260,6 +275,19 @@ cmd_calibrate() {
|
||||
echo " motion v=${vel} T≥${mt}s settle=${st}s qd≤${qd} pin=${ph}x${ps}s"
|
||||
echo " poses=${WP}"
|
||||
echo " result → ${RES}"
|
||||
local grip_args=()
|
||||
if [[ -n "${grip_tool}" ]]; then
|
||||
if [[ "${HANDEYE_MODE}" == "eye_in_hand" ]]; then
|
||||
echo " gripper_tool=${grip_tool} tcp_xyz=[${grip_xyz}] （AX=XB 用 TCP，T_4x4 折成 cam→法兰）"
|
||||
else
|
||||
echo " gripper_tool=${grip_tool} tcp_xyz=[${grip_xyz}] （AX=XB 用 TCP，T_4x4 仍是 cam→基座，不折）"
|
||||
fi
|
||||
grip_args+=(
|
||||
-p gripper_tool:="${grip_tool}"
|
||||
-p gripper_tcp_xyz:="[${grip_xyz}]"
|
||||
-p gripper_tcp_qwxyz:="[${grip_q}]"
|
||||
)
|
||||
fi
|
||||
exec ros2 run arm_application handeye_calibrate --ros-args \
|
||||
--params-file "${CAL}" \
|
||||
--params-file "${WP}" \
|
||||
@@ -280,7 +308,8 @@ cmd_calibrate() {
|
||||
-p settle_timeout_sec:="${sto}" \
|
||||
-p pin_holds:="${ph}" \
|
||||
-p pin_hold_sec:="${ps}" \
|
||||
-p skip_unstable_poses:="${skip}"
|
||||
-p skip_unstable_poses:="${skip}" \
|
||||
"${grip_args[@]}"
|
||||
}
|
||||
|
||||
cmd_waypoints() {
|
||||
@@ -359,6 +388,7 @@ cmd_teach() {
|
||||
-p joint_prefix:="${PREFIX}"
|
||||
-p camera_source:="${CAM}"
|
||||
-p output_dir:="${WORKDIR}"
|
||||
-p poses_file:="${TAUGHT}"
|
||||
-p target_points:="${points}"
|
||||
)
|
||||
if [[ -n "${CAM_SERIAL}" ]]; then
|
||||
|
||||
Reference in New Issue
Block a user