左臂换成 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
@@ -33,8 +33,11 @@ from arm_application.handeye_lib import (
|
||||
BoardConfig,
|
||||
CameraIntrinsics,
|
||||
ChessboardStabilizer,
|
||||
T_from_flexiv_tcp,
|
||||
calibrate_hand_eye,
|
||||
cam_to_flange_via_tool,
|
||||
detect_chessboard,
|
||||
format_matrix_yaml,
|
||||
format_transform_yaml,
|
||||
is_pose_filled,
|
||||
pose_to_Rt,
|
||||
@@ -113,11 +116,10 @@ class HandeyeCalibrate(TolerantNode):
|
||||
|
||||
# 棋盘（可自定义）
|
||||
# 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
|
||||
# 真实使用的板：内角点 8x11，方格 15mm（见 handeye/results/*.yaml 的头部注释）。
|
||||
# 这里原先写的是 9x6 / 0.025（另一张早期的板），一旦 site.yaml 漏配就会被
|
||||
# 静默启用 —— 症状是"标定能顺利跑完，但结果明显不对"，极难排查。
|
||||
self.declare_parameter('board_cols', 8) # 内角点
|
||||
self.declare_parameter('board_rows', 11)
|
||||
# 真实使用的板：A5 内角点 11x8，方格 15mm（site.yaml handeye.boards.eye_in_hand）。
|
||||
# 这里原先写的是 9x6 / 0.025，后来是 8x11；漏配就会静默用错尺寸。
|
||||
self.declare_parameter('board_cols', 11) # 内角点
|
||||
self.declare_parameter('board_rows', 8)
|
||||
self.declare_parameter('square_size_m', 0.015)
|
||||
|
||||
self.declare_parameter('stable_window', 8)
|
||||
@@ -138,6 +140,11 @@ class HandeyeCalibrate(TolerantNode):
|
||||
self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
|
||||
self.declare_parameter('result_file', '')
|
||||
self.declare_parameter('cleanup_images', True)
|
||||
# 眼在手上：AX=XB 的 gripper 用 Studio 工具 TCP，再折成 cam→法兰写 T_4x4。
|
||||
# 空 = 旧口径（ee_pose 当 gripper，T_4x4 原样写出）。配了就必须带 TCP。
|
||||
self.declare_parameter('gripper_tool', '')
|
||||
self.declare_parameter('gripper_tcp_xyz', [0.0, 0.0, 0.0])
|
||||
self.declare_parameter('gripper_tcp_qwxyz', [1.0, 0.0, 0.0, 0.0])
|
||||
|
||||
prefix = str(self.get_parameter('joint_prefix').value or '')
|
||||
base = list(
|
||||
@@ -152,6 +159,21 @@ class HandeyeCalibrate(TolerantNode):
|
||||
self.start_index = max(0, int(self.get_parameter('start_index').value))
|
||||
self.min_poses = int(self.get_parameter('min_poses').value)
|
||||
self.cleanup_images = bool(self.get_parameter('cleanup_images').value)
|
||||
self.gripper_tool = str(self.get_parameter('gripper_tool').value or '').strip()
|
||||
tcp_xyz = list(
|
||||
self.get_parameter('gripper_tcp_xyz').get_parameter_value().double_array_value
|
||||
or [0.0, 0.0, 0.0]
|
||||
)
|
||||
tcp_q = list(
|
||||
self.get_parameter('gripper_tcp_qwxyz').get_parameter_value().double_array_value
|
||||
or [1.0, 0.0, 0.0, 0.0]
|
||||
)
|
||||
self.T_flange_to_tool = T_from_flexiv_tcp(tcp_xyz[:3], tcp_q[:4])
|
||||
if self.gripper_tool and float(np.linalg.norm(self.T_flange_to_tool[:3, 3])) < 1e-4:
|
||||
raise RuntimeError(
|
||||
f'gripper_tool={self.gripper_tool!r} 但 gripper_tcp_xyz 几乎是零：'
|
||||
'等于没把 Studio TCP 传进来。不许按法兰口径假装在用工具系。'
|
||||
)
|
||||
|
||||
self.camera_source = str(self.get_parameter('camera_source').value).strip().lower()
|
||||
self.camera_serial = str(self.get_parameter('camera_serial').value or '').strip()
|
||||
@@ -265,10 +287,12 @@ class HandeyeCalibrate(TolerantNode):
|
||||
self.images_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
eye_to_hand = self.mode == 'eye_to_hand'
|
||||
grip = self.gripper_tool or '（ee_pose 原样，无工具折算）'
|
||||
self.get_logger().info(
|
||||
f'手眼标定 [{self.mode}] camera_source={self.camera_source}：'
|
||||
f'{len(poses)} 个位姿，棋盘={self.board.cols}x{self.board.rows} '
|
||||
f'方格={self.board.square_size}m，v={self.velocity_scale}'
|
||||
f'方格={self.board.square_size}m，v={self.velocity_scale} '
|
||||
f'gripper_tool={grip}'
|
||||
)
|
||||
self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。')
|
||||
|
||||
@@ -368,12 +392,58 @@ class HandeyeCalibrate(TolerantNode):
|
||||
|
||||
self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...')
|
||||
R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand)
|
||||
name = (
|
||||
'T_cam_to_base (eye_to_hand)'
|
||||
if eye_to_hand
|
||||
else 'T_cam_to_gripper (eye_in_hand)'
|
||||
)
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
extra = ''
|
||||
if eye_to_hand:
|
||||
name = 'T_cam_to_base (eye_to_hand)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
if self.gripper_tool:
|
||||
extra = (
|
||||
f'gripper_tool: {self.gripper_tool}\n'
|
||||
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'gripper={self.gripper_tool}：AX=XB 的手是 TCP；'
|
||||
f'T_4x4 是 cam→基座，不折 t={t.reshape(3)*1000} mm'
|
||||
)
|
||||
elif self.gripper_tool:
|
||||
T_cam_tool = np.eye(4)
|
||||
T_cam_tool[:3, :3] = R
|
||||
T_cam_tool[:3, 3] = t.reshape(3)
|
||||
T_cam_flange = cam_to_flange_via_tool(T_cam_tool, self.T_flange_to_tool)
|
||||
delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
|
||||
tool_len = float(np.linalg.norm(self.T_flange_to_tool[:3, 3]))
|
||||
if tool_len > 0.05 and delta < 1e-4:
|
||||
self.get_logger().error(
|
||||
'T_cam_to_flange 与 T_cam_to_tool 平移几乎相同，'
|
||||
'折算没生效。结果作废。'
|
||||
)
|
||||
return 11
|
||||
# 腕部相机在法兰前方（法兰 +Z）。乘反了 Z 会落到 −200 mm 一带。
|
||||
if (
|
||||
self.T_flange_to_tool[2, 3] > 0.05
|
||||
and float(T_cam_flange[2, 3]) < 0.0
|
||||
):
|
||||
self.get_logger().error(
|
||||
f'T_cam_to_flange tz={float(T_cam_flange[2, 3])*1000:.1f} mm '
|
||||
'在法兰后方，乘序反了。结果作废。'
|
||||
)
|
||||
return 12
|
||||
R, t = T_cam_flange[:3, :3], T_cam_flange[:3, 3].reshape(3, 1)
|
||||
name = 'T_cam_to_flange (eye_in_hand，由工具 TCP 折算)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
extra = (
|
||||
f'gripper_tool: {self.gripper_tool}\n'
|
||||
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
|
||||
+ format_matrix_yaml(T_cam_tool, 'T_cam_to_tool')
|
||||
)
|
||||
self.get_logger().info(
|
||||
f'gripper={self.gripper_tool}：'
|
||||
f'T_cam_to_tool t={T_cam_tool[:3, 3]*1000} mm → '
|
||||
f'T_cam_to_flange t={T_cam_flange[:3, 3]*1000} mm'
|
||||
)
|
||||
else:
|
||||
name = 'T_cam_to_gripper (eye_in_hand)'
|
||||
yaml_txt = format_transform_yaml(R, t, name)
|
||||
header = (
|
||||
f'# 手眼标定结果\n'
|
||||
f'# mode: {self.mode}\n'
|
||||
@@ -383,8 +453,20 @@ class HandeyeCalibrate(TolerantNode):
|
||||
f'# board: {self.board.cols}x{self.board.rows} '
|
||||
f'square_m={self.board.square_size}\n'
|
||||
)
|
||||
if self.gripper_tool and eye_to_hand:
|
||||
header += (
|
||||
f'# gripper_tool: {self.gripper_tool}\n'
|
||||
f'# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）\n'
|
||||
f'# 旧结果备份在 results/backup/\n'
|
||||
)
|
||||
elif self.gripper_tool:
|
||||
header += (
|
||||
f'# gripper_tool: {self.gripper_tool}\n'
|
||||
f'# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）\n'
|
||||
f'# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/\n'
|
||||
)
|
||||
self.result_file.parent.mkdir(parents=True, exist_ok=True)
|
||||
self.result_file.write_text(header + yaml_txt, encoding='utf-8')
|
||||
self.result_file.write_text(header + extra + yaml_txt, encoding='utf-8')
|
||||
(self.output_dir / 'captures.json').write_text(
|
||||
json.dumps(meta, indent=2), encoding='utf-8'
|
||||
)
|
||||
|
||||
@@ -342,6 +342,38 @@ def calibrate_hand_eye(
|
||||
return calibrate_hand_eye_tsai(Rg, tg, R_target2cam, t_target2cam)
|
||||
|
||||
|
||||
def T_from_flexiv_tcp(xyz_m, qwxyz) -> np.ndarray:
|
||||
"""Studio ``tcp_location`` → 法兰→工具 4×4。
|
||||
|
||||
Flexiv 顺序是 ``[x, y, z, q_w, q_x, q_y, q_z]``，四元数是 wxyz，
|
||||
和本仓库其余地方的 xyzw 不一样。只在这里换一次。
|
||||
"""
|
||||
xyz = np.asarray(xyz_m, dtype=float).reshape(3)
|
||||
q = np.asarray(qwxyz, dtype=float).reshape(7) if np.asarray(qwxyz).size == 7 else np.asarray(qwxyz, dtype=float).reshape(4)
|
||||
if q.size == 7:
|
||||
qw, qx, qy, qz = (float(q[3]), float(q[4]), float(q[5]), float(q[6]))
|
||||
else:
|
||||
qw, qx, qy, qz = (float(q[0]), float(q[1]), float(q[2]), float(q[3]))
|
||||
R = quat_xyzw_to_R((qx, qy, qz, qw))
|
||||
return Rt_to_T(R, xyz.reshape(3, 1))
|
||||
|
||||
|
||||
def cam_to_flange_via_tool(T_cam_to_tool: np.ndarray, T_flange_to_tool: np.ndarray) -> np.ndarray:
|
||||
"""AX=XB 在工具 TCP 上解出 cam→工具后，再折成 cam→法兰。
|
||||
|
||||
``T_flange_to_tool`` 是 Studio TCP：``p_法兰 = T_flange_to_tool · p_工具``。
|
||||
OpenCV 的 ``T_cam_to_tool`` 是 ``p_工具 = T_cam_to_tool · p_相机``。
|
||||
所以 ``T_cam_to_flange = T_flange_to_tool · T_cam_to_tool``。
|
||||
|
||||
乘反了（``T_cam_to_tool · inv(T_ft)``）旋转看着还像，平移会把相机
|
||||
扔到法兰后方约 30 cm。忘了乘则 ``T_4x4`` 停在 TCP 上，沿工具偏 ~225 mm。
|
||||
两种都不报错。
|
||||
"""
|
||||
T_ct = np.asarray(T_cam_to_tool, dtype=float).reshape(4, 4)
|
||||
T_ft = np.asarray(T_flange_to_tool, dtype=float).reshape(4, 4)
|
||||
return T_ft @ T_ct
|
||||
|
||||
|
||||
def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
|
||||
T = Rt_to_T(R, t)
|
||||
lines = [
|
||||
@@ -354,6 +386,14 @@ def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
|
||||
return '\n'.join(lines) + '\n'
|
||||
|
||||
|
||||
def format_matrix_yaml(T: np.ndarray, key: str) -> str:
|
||||
arr = np.asarray(T, dtype=float).reshape(4, 4)
|
||||
lines = [f'{key}:']
|
||||
for row in arr:
|
||||
lines.append(' - [' + ', '.join(f'{v:.8f}' for v in row) + ']')
|
||||
return '\n'.join(lines) + '\n'
|
||||
|
||||
|
||||
def is_pose_filled(joints: Sequence[float]) -> bool:
|
||||
"""若全为零 / NaN 占位则返回 False。"""
|
||||
arr = np.asarray(joints, dtype=float)
|
||||
|
||||
@@ -122,6 +122,8 @@ class RdkBridge:
|
||||
'PATH': f'{Path(py).parent}:/usr/bin:/bin',
|
||||
'LD_LIBRARY_PATH': flexiv_lib,
|
||||
'LANG': 'C.UTF-8',
|
||||
'SITE_PC_IP': os.environ.get('SITE_PC_IP', ''),
|
||||
'SITE_NET_IFACE': os.environ.get('SITE_NET_IFACE', ''),
|
||||
}
|
||||
# 不要继承 conda 的 PYTHONPATH。
|
||||
# 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁
|
||||
@@ -254,6 +256,9 @@ class HandeyeTeach(TolerantNode):
|
||||
# 设了就只是个"提醒"：采够时提示可以收工，不强制关窗 ——
|
||||
# 常常采到一半发现某个角度还没覆盖，强制退出反而要重开。
|
||||
self.declare_parameter('target_points', 0)
|
||||
# 必须和 run_handeye.sh 的 TAUGHT 是同一个文件。只写 taught_poses.txt、
|
||||
# 脚本去读 taught_poses_<模式>.txt，merge 就会「文件在、内容空」。
|
||||
self.declare_parameter('poses_file', '')
|
||||
|
||||
self.backend = str(self.get_parameter('backend').value).strip().lower()
|
||||
self.robot_sn = str(self.get_parameter('robot_sn').value)
|
||||
@@ -261,8 +266,12 @@ class HandeyeTeach(TolerantNode):
|
||||
|
||||
self.output_dir = Path(str(self.get_parameter('output_dir').value)).expanduser()
|
||||
self.images_dir = self.output_dir / 'images'
|
||||
self.poses_file = self.output_dir / 'taught_poses.txt'
|
||||
_pf = str(self.get_parameter('poses_file').value).strip()
|
||||
self.poses_file = (
|
||||
Path(_pf).expanduser() if _pf else self.output_dir / 'taught_poses.txt'
|
||||
)
|
||||
self.yaml_file = self.output_dir / 'taught_waypoint_positions.yaml'
|
||||
self._last_capture_t = 0.0
|
||||
self.images_dir.mkdir(parents=True, exist_ok=True)
|
||||
|
||||
self._captures: List[dict] = []
|
||||
@@ -531,6 +540,20 @@ class HandeyeTeach(TolerantNode):
|
||||
if q is None:
|
||||
self.get_logger().error('读取关节失败')
|
||||
return
|
||||
now = time.monotonic()
|
||||
if now - self._last_capture_t < 0.8:
|
||||
self.get_logger().warn('按太快，忽略（防 Enter 连发）')
|
||||
return
|
||||
if self._captures:
|
||||
last = self._captures[-1]['joints']
|
||||
delta = max(abs(a - b) for a, b in zip(q, last))
|
||||
if delta < 0.015:
|
||||
self.get_logger().warn(
|
||||
f'关节几乎没动（Δ={delta:.4f} rad），忽略这次拍照。'
|
||||
'先拖到另一个姿态再按 Enter。'
|
||||
)
|
||||
return
|
||||
self._last_capture_t = now
|
||||
|
||||
idx = len(self._captures)
|
||||
stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
|
||||
|
||||
@@ -17,6 +17,32 @@ from typing import Iterable, Optional
|
||||
import numpy as np
|
||||
|
||||
from arm_application.handeye_lib import cam_point_to_base
|
||||
from arm_application.path_profile import ( # 伺服和测试从本模块再导出，不另抄一份
|
||||
D435_FULL_ACC_M,
|
||||
D435_FULL_CRUISE_MPS,
|
||||
D435_FULL_DEC_M,
|
||||
D435_MID_HOP_MAX_M,
|
||||
D435_MID_LEFTOVER_M,
|
||||
D435_MOVE_ACC_MPS2,
|
||||
D435_MOVE_SPEED_SCALE,
|
||||
D435_NEAR_CRUISE_MPS,
|
||||
D435_NEAR_HOP_MAX_M,
|
||||
D435_NEAR_LEFTOVER_M,
|
||||
D435_VERT_TILT_RAD,
|
||||
D435_SETTLE_S,
|
||||
FROZEN_J_HOP_MAX_M,
|
||||
build_arc_xy_move,
|
||||
flange_down_w,
|
||||
flange_tilt_rad,
|
||||
hybrid_cruise_vxy,
|
||||
oneshot_hop_limits,
|
||||
smooth_joint_schedule,
|
||||
trap_acc_for_hop,
|
||||
translate_hold_ori_waypoints,
|
||||
)
|
||||
|
||||
# 上一跳实际少走这么多，就认定冻结 J 在当前姿态走不动，不许按同一剩差再刷。
|
||||
D435_MIN_PROGRESS_FRAC = 0.40
|
||||
|
||||
|
||||
def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
|
||||
@@ -30,6 +56,301 @@ def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
|
||||
return max(vals)
|
||||
|
||||
|
||||
def z_locked_qos():
|
||||
"""``visual_servo/z_locked`` 的 QoS。发布端和订阅端必须用这一份。
|
||||
|
||||
感知比伺服晚启动。默认 volatile 会丢掉伺服启动时已经发出的「已到锁高」，
|
||||
感知就停在初值 False，D435 永远不拍。
|
||||
"""
|
||||
from rclpy.qos import ( # noqa: PLC0415 — 纯函数测试不需要在导入时拉起 ROS
|
||||
DurabilityPolicy,
|
||||
HistoryPolicy,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
)
|
||||
|
||||
return QoSProfile(
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=1,
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
||||
)
|
||||
|
||||
|
||||
def at_lock_height(z_m: float, lock_z_m: float, gate_m: float) -> bool:
|
||||
"""法兰是否在锁高带内。
|
||||
|
||||
低于锁高**不算**到位。上一轮观测下降停在 0.26 m 时，若把「不高于锁高」
|
||||
当成到位，XY 会在低位开跑，D435 又因高度不对拒绝拍照，两边都不再动。
|
||||
``lock_z_m <= 0`` 表示没启用锁高。
|
||||
"""
|
||||
lock = float(lock_z_m)
|
||||
if lock <= 0.0:
|
||||
return True
|
||||
gate = max(0.02, float(gate_m))
|
||||
return abs(float(z_m) - lock) <= gate
|
||||
|
||||
|
||||
def lock_z_velocity(z_m: float, lock_z_m: float, *, kp: float, v_lim: float) -> float:
|
||||
"""回到锁高的竖直速度 [m/s]，基座 +Z 朝上。
|
||||
|
||||
低于锁高为正（上抬），高于为负（下降）。4 mm 内视为到位。
|
||||
不许在低于锁高时把速度写成 0：那会和「没到锁高不拍 D435」卡死。
|
||||
"""
|
||||
lock = float(lock_z_m)
|
||||
if lock <= 0.0:
|
||||
return 0.0
|
||||
ez = lock - float(z_m)
|
||||
if abs(ez) < 0.004:
|
||||
return 0.0
|
||||
lim = abs(float(v_lim))
|
||||
return float(np.clip(float(kp) * ez, -lim, lim))
|
||||
|
||||
|
||||
def hold_translation_vz(z_m: float, z_hold_m: float, *, kp: float, v_lim: float) -> float:
|
||||
"""平移时把法兰钉在**这一段开始时的高度**。
|
||||
|
||||
只写 v_z=0 不够：大范围平移加姿态校正时，阻尼求逆会把竖直速度吃掉，
|
||||
2026-10-09 真机从 0.327 m 漂到 0.307 m 又抬到 0.337 m。这里用开始那一
|
||||
帧的高度当设定，4 mm 内不动，避免来回抖。不追 site 里的锁高数字，
|
||||
也不因此停掉 XY。下臂仍只在二阶段。
|
||||
"""
|
||||
ez = float(z_hold_m) - float(z_m)
|
||||
if abs(ez) < 0.004:
|
||||
return 0.0
|
||||
lim = abs(float(v_lim))
|
||||
return float(np.clip(float(kp) * ez, -lim, lim))
|
||||
|
||||
|
||||
# D405 连续这么久没有高置信度目标，才让待命的 D435 拍一张。
|
||||
# 单帧漏检就拍，会在平移半路把目标换掉。
|
||||
D405_MISS_BEFORE_D435_S = 0.5
|
||||
# 拍了但伺服没 ``d435_commanding``（工作盒拒收等）：超过这时限就允许再拍。
|
||||
# 不清的话 ``_oneshot_shot`` 会永远真，臂停着、感知还以为在赶路。
|
||||
ONESHOT_ACCEPT_TIMEOUT_S = 0.8
|
||||
|
||||
|
||||
def oneshot_shot_timed_out(
|
||||
*,
|
||||
shot: bool,
|
||||
commanding: bool,
|
||||
age_s: float,
|
||||
timeout_s: float = ONESHOT_ACCEPT_TIMEOUT_S,
|
||||
) -> bool:
|
||||
"""拍过一张、伺服却没接住 → True。已经在赶路不许清。"""
|
||||
if not bool(shot) or bool(commanding):
|
||||
return False
|
||||
return float(age_s) >= float(timeout_s)
|
||||
|
||||
|
||||
def d405_commands_arm(
|
||||
*,
|
||||
using_d435: bool,
|
||||
arrived: bool,
|
||||
settled: bool,
|
||||
move_only: bool = False,
|
||||
) -> bool:
|
||||
"""D405 这一帧的点能不能下发给左臂。
|
||||
|
||||
推理一直开着。正在用 D435 那一个点时，点到并且停稳之前不下发。
|
||||
停稳沿用一阶段的 ``StillLock``（site 里的 ``lock_still_sec`` /
|
||||
``lock_still_move_m``），不另写一套计时。
|
||||
``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
|
||||
"""
|
||||
if move_only:
|
||||
return False
|
||||
if not using_d435:
|
||||
return True
|
||||
return bool(arrived) and bool(settled)
|
||||
|
||||
|
||||
def oneshot_needs_verticalize(
|
||||
tilt_rad: float,
|
||||
leftover_m: float,
|
||||
arrive_m: float,
|
||||
*,
|
||||
tilt_lim: float = D435_VERT_TILT_RAD,
|
||||
already_done: bool = False,
|
||||
) -> bool:
|
||||
"""XY 已经进到位门槛、倾角还大，才单独收垂直。不许和平移捆在一起。"""
|
||||
if already_done:
|
||||
return False
|
||||
if float(leftover_m) > float(arrive_m):
|
||||
return False
|
||||
return float(tilt_rad) > float(tilt_lim)
|
||||
|
||||
|
||||
def oneshot_next_hop_m(
|
||||
dist_m: float, hop_max_m: float = FROZEN_J_HOP_MAX_M
|
||||
) -> float:
|
||||
"""这一跳最多走 ``hop_max_m``。冻结 J 一次规划 400 mm 会少走一大截。"""
|
||||
return min(max(0.0, float(dist_m)), float(hop_max_m))
|
||||
|
||||
|
||||
def oneshot_cartesian_dp(
|
||||
p_ee,
|
||||
goal_xy,
|
||||
lock_z: float,
|
||||
*,
|
||||
gate_m: float,
|
||||
hop_max_m: Optional[float] = None,
|
||||
leftover_xy_m: Optional[float] = None,
|
||||
):
|
||||
"""这一跳的笛卡尔位移。没到锁高只收 Z；远处 XY 带锁高；近处只走 XY。
|
||||
|
||||
工件上方最后一跳若再把 7 mm 的 Z 和 60 mm 的 XY 捆在一起，冻结 J 会把腕部带偏。
|
||||
远处仍带 ``ee_z_m``，避免长跳把高度抬出锁高带。
|
||||
"""
|
||||
p = np.asarray(p_ee, dtype=float).reshape(3)
|
||||
z_lock = float(lock_z)
|
||||
leftover = (
|
||||
float(leftover_xy_m)
|
||||
if leftover_xy_m is not None
|
||||
else float(
|
||||
np.hypot(float(goal_xy[0]) - float(p[0]), float(goal_xy[1]) - float(p[1]))
|
||||
)
|
||||
)
|
||||
hop_cap = (
|
||||
float(hop_max_m)
|
||||
if hop_max_m is not None
|
||||
else oneshot_hop_limits(leftover)[0]
|
||||
)
|
||||
if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
|
||||
dz = z_lock - float(p[2])
|
||||
mag = oneshot_next_hop_m(abs(dz), hop_cap)
|
||||
if mag < 1e-6:
|
||||
return np.zeros(3, dtype=float)
|
||||
return np.array([0.0, 0.0, float(np.sign(dz)) * mag], dtype=float)
|
||||
dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
|
||||
dp = np.array(
|
||||
[
|
||||
float(goal_xy[0]) - float(p[0]),
|
||||
float(goal_xy[1]) - float(p[1]),
|
||||
dz,
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
n = float(np.linalg.norm(dp))
|
||||
hop = oneshot_next_hop_m(n, hop_cap)
|
||||
if n > hop > 1e-9:
|
||||
dp = dp * (hop / n)
|
||||
return dp
|
||||
|
||||
|
||||
def oneshot_full_segments(
|
||||
p_ee,
|
||||
goal_xy,
|
||||
lock_z: float,
|
||||
*,
|
||||
gate_m: float,
|
||||
):
|
||||
"""整段一次走完的笛卡尔分段。先收齐锁高，再走全部 XY，不切 80/15 mm。
|
||||
|
||||
近处（剩差 ≤80 mm）仍然只走 XY，不混 Z。收垂直不在这里。
|
||||
返回 ``[(dp, tag), ...]``，``tag`` 是 ``lift_z`` 或 ``xy``。
|
||||
"""
|
||||
p = np.asarray(p_ee, dtype=float).reshape(3).copy()
|
||||
z_lock = float(lock_z)
|
||||
goal = np.asarray(goal_xy, dtype=float).reshape(2)
|
||||
out = []
|
||||
if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
|
||||
dz = z_lock - float(p[2])
|
||||
if abs(dz) >= 1e-6:
|
||||
out.append((np.array([0.0, 0.0, dz], dtype=float), "lift_z"))
|
||||
p[2] = z_lock
|
||||
leftover = float(np.hypot(float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1])))
|
||||
dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
|
||||
dp = np.array(
|
||||
[float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1]), dz],
|
||||
dtype=float,
|
||||
)
|
||||
if float(np.linalg.norm(dp)) >= 1e-6:
|
||||
out.append((dp, "xy"))
|
||||
return out
|
||||
|
||||
|
||||
def oneshot_concat_hold_ori(q0, J, segments, *, R=None):
|
||||
"""把分段 ``dp`` 用同一份冻结 J 串成一条关节折线。中途不回零速。"""
|
||||
q = np.asarray(q0, dtype=float).copy()
|
||||
wps = [q.copy()]
|
||||
planned = 0.0
|
||||
asked = 0.0
|
||||
tags = []
|
||||
for dp, tag in segments:
|
||||
hop = float(np.linalg.norm(np.asarray(dp, dtype=float).reshape(3)))
|
||||
if hop < 1e-6:
|
||||
continue
|
||||
asked += hop
|
||||
part, remain = translate_hold_ori_waypoints(
|
||||
q, J, dp, R=R, verticalize=False
|
||||
)
|
||||
if len(part) < 2:
|
||||
break
|
||||
wps.extend(part[1:])
|
||||
q = wps[-1]
|
||||
planned += hop - float(remain)
|
||||
tags.append(str(tag))
|
||||
tag = "full"
|
||||
if tags == ["lift_z"]:
|
||||
tag = "lift_z"
|
||||
elif "lift_z" in tags and "xy" in tags:
|
||||
tag = "full"
|
||||
elif tags == ["xy"]:
|
||||
tag = "full"
|
||||
return wps, planned, asked, tag
|
||||
|
||||
|
||||
def oneshot_full_cruise_mps(leftover_m: float, far_cruise_mps: float) -> float:
|
||||
"""整段巡航：工件上方 30 mm/s；远处封顶 ``D435_FULL_CRUISE_MPS``，不许再弹到 80。"""
|
||||
far = max(1e-4, float(far_cruise_mps))
|
||||
if float(leftover_m) <= D435_NEAR_LEFTOVER_M:
|
||||
return min(D435_NEAR_CRUISE_MPS, far)
|
||||
return min(D435_FULL_CRUISE_MPS, far)
|
||||
|
||||
|
||||
def oneshot_may_send_another(
|
||||
leftover_m: float,
|
||||
last_dist_m: Optional[float],
|
||||
*,
|
||||
arrive_m: float,
|
||||
hop_max_m: float = FROZEN_J_HOP_MAX_M,
|
||||
min_progress_frac: float = D435_MIN_PROGRESS_FRAC,
|
||||
) -> bool:
|
||||
"""上一跳走完后还要不要再发一条。
|
||||
|
||||
剩差已经进到位门槛 → 钉住。
|
||||
上一跳几乎没缩短剩差 → 不许按同一距离再刷（真机 28 mm 死循环）。
|
||||
"""
|
||||
if float(leftover_m) <= float(arrive_m):
|
||||
return False
|
||||
if last_dist_m is None:
|
||||
return True
|
||||
progressed = float(last_dist_m) - float(leftover_m)
|
||||
planned = min(float(last_dist_m), float(hop_max_m))
|
||||
if progressed < float(min_progress_frac) * max(planned, 1e-6):
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def oneshot_should_submit(
|
||||
*,
|
||||
handoff: bool,
|
||||
low_for_s: float,
|
||||
already_shot: bool,
|
||||
wait_s: float = D405_MISS_BEFORE_D435_S,
|
||||
) -> bool:
|
||||
"""腕部连续 ``wait_s`` 秒没有高置信度目标、这一轮还没拍成，才让 D435 拍一张。
|
||||
|
||||
高度不在这里再判一次。进一阶段之前已经抬到锁高；再等伺服报「已到锁高」
|
||||
会和「没到锁高就只收 Z、不平移」卡死。拍成一张之后不许连拍，顶视不是第二套跟踪。
|
||||
"""
|
||||
return (
|
||||
bool(handoff)
|
||||
and (not already_shot)
|
||||
and float(low_for_s) >= float(wait_s)
|
||||
)
|
||||
|
||||
|
||||
def select_oneshot_det(dets: Optional[Iterable], min_conf: float):
|
||||
"""顶视一次性：只收 conf≥门槛的框，取最高分。没有就 None，不许降级。"""
|
||||
if not dets:
|
||||
|
||||
@@ -0,0 +1,550 @@
|
||||
"""路径弧长上的梯形速度，以及 XY 圆弧。
|
||||
|
||||
二阶段下臂和一阶段 D435 迁臂共用这一份。不许再抄一套加减速。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
# 整段同一巡航：site ``v_slow_xy`` × 本系数。1.0 → 80 mm/s（二阶段对孔同档上限）。
|
||||
D435_MOVE_SPEED_SCALE = 1.0
|
||||
# 缺路程时的兜底加速度。真机 hop 用 ``trap_acc_for_hop``，和二阶段同一条刹车。
|
||||
D435_MOVE_ACC_MPS2 = 0.125
|
||||
# action 走完后再停这么久，才重测 TF。二阶段 ``_goto`` 的 settle_s。
|
||||
D435_SETTLE_S = 0.25
|
||||
# 冻结雅可比一次只信这么远。二阶段 ``_plan_and_send`` 默认 ``max_seg`` 同源。
|
||||
# 400 mm 一次规划，真机剩 117 mm，随后 28 mm 死循环刷轨迹。
|
||||
FROZEN_J_HOP_MAX_M = 0.08
|
||||
# 剩差进入这一带 = 工件上方。真机最后一跳 37–75 mm @ 80 mm/s 会晃并偏转。
|
||||
D435_NEAR_LEFTOVER_M = 0.08
|
||||
D435_NEAR_HOP_MAX_M = 0.015
|
||||
D435_NEAR_CRUISE_MPS = 0.030
|
||||
# 法兰倾角超过这个才单独收垂直。和 XY 捆在同一跳会把腕部漏成 Y 过冲。
|
||||
D435_VERT_TILT_RAD = 0.015
|
||||
D435_MID_LEFTOVER_M = 0.15
|
||||
D435_MID_HOP_MAX_M = 0.040
|
||||
# 弦长的 12% 当矢高，封顶 50 mm，避免大圆弧甩出工作盒。
|
||||
D435_ARC_SAGITTA_FRAC = 0.12
|
||||
D435_ARC_SAGITTA_MAX_M = 0.05
|
||||
# 整段一次走完。起步必须够长：40 mm 就爬到 80 mm/s 是真机那下弹射
|
||||
# （头尾速度本来就是 0，不是初速度；平移也不下发角速度）。
|
||||
# 峰值加速度 a_max = π v² / (4 d_acc)；100 mm @ 60 mm/s → 约 0.028 m/s²。
|
||||
D435_FULL_ACC_M = 0.100
|
||||
D435_FULL_DEC_M = 0.080
|
||||
D435_FULL_CRUISE_MPS = 0.060
|
||||
|
||||
|
||||
def hybrid_cruise_vxy(
|
||||
v_slow_xy: float, scale: float = D435_MOVE_SPEED_SCALE
|
||||
) -> float:
|
||||
"""远处巡航。近处由 ``oneshot_hop_limits`` 另给一档，不许再抄 15 mm/s。"""
|
||||
return max(1e-4, float(v_slow_xy) * float(scale))
|
||||
|
||||
|
||||
def oneshot_hop_limits(
|
||||
leftover_m: float, far_cruise_mps: float = 0.08
|
||||
) -> Tuple[float, float]:
|
||||
"""这一跳的 ``(hop_max_m, cruise_mps)``。近处必须比 80 mm / 80 mm/s 短且慢。"""
|
||||
d = max(0.0, float(leftover_m))
|
||||
far = max(1e-4, float(far_cruise_mps))
|
||||
if d <= D435_NEAR_LEFTOVER_M:
|
||||
return D435_NEAR_HOP_MAX_M, min(D435_NEAR_CRUISE_MPS, far)
|
||||
if d <= D435_MID_LEFTOVER_M:
|
||||
return D435_MID_HOP_MAX_M, far
|
||||
return FROZEN_J_HOP_MAX_M, far
|
||||
|
||||
|
||||
def flange_down_w(
|
||||
R,
|
||||
*,
|
||||
dead_rad: float = 0.006,
|
||||
gain: float = 2.0,
|
||||
mag_lim: float = 0.5,
|
||||
) -> Tuple[np.ndarray, float]:
|
||||
"""让法兰 +Z 指向世界 −Z 的角速度 + 当前倾角。
|
||||
|
||||
和二阶段 ``_w_down`` 同一份。死区 0.006 rad（约 0.34°）以下不再修。
|
||||
"""
|
||||
z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
|
||||
z_des = np.array([0.0, 0.0, -1.0], dtype=float)
|
||||
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
|
||||
ang = float(math.acos(c))
|
||||
if ang < float(dead_rad):
|
||||
return np.zeros(3, dtype=float), ang
|
||||
axis = np.cross(z_now, z_des)
|
||||
n = float(np.linalg.norm(axis))
|
||||
axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0], dtype=float)
|
||||
mag = float(np.clip(float(gain) * ang, -float(mag_lim), float(mag_lim)))
|
||||
return axis * mag, ang
|
||||
|
||||
|
||||
def flange_tilt_rad(R) -> float:
|
||||
"""法兰 +Z 相对世界 −Z 的倾角 [rad]。只读角度，不带死区。"""
|
||||
z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
|
||||
z_des = np.array([0.0, 0.0, -1.0], dtype=float)
|
||||
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
|
||||
return float(math.acos(c))
|
||||
|
||||
|
||||
def trap_acc_for_hop(dist: float, v: float) -> float:
|
||||
"""二阶段 ``_plan_and_send`` 同一条刹车：路程越长刹车越远，不许另写一套。"""
|
||||
v_cmd = max(min(float(v), 0.08), 1e-3)
|
||||
d_brake = min(0.030, max(0.006, 0.32 * max(float(dist), 1e-6)))
|
||||
return max(v_cmd * v_cmd / (2.0 * d_brake), 0.030)
|
||||
|
||||
|
||||
def trap_params(dist: float, v: float, a: float) -> Tuple[float, float, float, float, float]:
|
||||
"""Rest-to-rest 梯形：返回 (T, v_peak, t_acc, t_cruise, a)。巡航速度保持 v。"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
v = max(float(v), 1e-4)
|
||||
a = max(float(a), 1e-3)
|
||||
d_acc = v * v / (2.0 * a)
|
||||
if 2.0 * d_acc >= dist:
|
||||
v_peak = math.sqrt(max(a * dist, 1e-12))
|
||||
t_acc = v_peak / a
|
||||
return 2.0 * t_acc, v_peak, t_acc, 0.0, a
|
||||
t_acc = v / a
|
||||
t_cruise = (dist - 2.0 * d_acc) / v
|
||||
return 2.0 * t_acc + t_cruise, v, t_acc, t_cruise, a
|
||||
|
||||
|
||||
def trap_sv(
|
||||
t: float, dist: float, v_peak: float, t_acc: float, t_cruise: float, a: float,
|
||||
) -> Tuple[float, float]:
|
||||
"""时刻 t 的路径弧长 s 与切向速度。"""
|
||||
t = max(0.0, float(t))
|
||||
t_dec0 = t_acc + t_cruise
|
||||
t_end = t_dec0 + t_acc
|
||||
if t >= t_end:
|
||||
return dist, 0.0
|
||||
if t < t_acc:
|
||||
return 0.5 * a * t * t, a * t
|
||||
if t < t_dec0:
|
||||
return 0.5 * a * t_acc * t_acc + v_peak * (t - t_acc), v_peak
|
||||
td = t - t_dec0
|
||||
s0 = 0.5 * a * t_acc * t_acc + v_peak * t_cruise
|
||||
return s0 + v_peak * td - 0.5 * a * td * td, max(0.0, v_peak - a * td)
|
||||
|
||||
|
||||
def smooth_path_params(
|
||||
dist: float,
|
||||
v: float,
|
||||
*,
|
||||
d_acc: float = D435_FULL_ACC_M,
|
||||
d_dec: float = D435_FULL_DEC_M,
|
||||
) -> Tuple[float, float, float, float, float, float]:
|
||||
"""整段一次走完的余弦加减速：(T, v_peak, t_acc, t_cruise, d_acc, d_dec)。
|
||||
|
||||
加速/减速段速度按半个余弦走，两端加速度为 0，没有梯形那种加速度台阶。
|
||||
路程不够巡航时整段一条余弦，峰值不超过 ``v``。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
v = max(float(v), 1e-4)
|
||||
acc = min(max(float(d_acc), 1e-4), 0.45 * dist)
|
||||
dec = min(max(float(d_dec), 1e-4), 0.45 * dist)
|
||||
if acc + dec >= dist - 1e-6:
|
||||
# 整段一条正弦速度：v = v_peak sin(π t / T)，s(T)=2 v_peak T / π。
|
||||
T = math.pi * dist / (2.0 * v)
|
||||
return T, v, 0.0, 0.0, 0.0, dist
|
||||
t_acc = 2.0 * acc / v
|
||||
t_dec = 2.0 * dec / v
|
||||
t_cruise = (dist - acc - dec) / v
|
||||
T = t_acc + t_cruise + t_dec
|
||||
return T, v, t_acc, t_cruise, acc, dec
|
||||
|
||||
|
||||
def cosine_peak_acc(v: float, d_acc: float) -> float:
|
||||
"""余弦加速段的峰值切向加速度 [m/s²]。``d_acc`` 越长，起步越不弹。"""
|
||||
d = max(float(d_acc), 1e-6)
|
||||
return math.pi * float(v) * float(v) / (4.0 * d)
|
||||
|
||||
|
||||
def smooth_path_sv(
|
||||
t: float,
|
||||
dist: float,
|
||||
v_peak: float,
|
||||
t_acc: float,
|
||||
t_cruise: float,
|
||||
d_acc: float,
|
||||
d_dec: float,
|
||||
) -> Tuple[float, float]:
|
||||
"""时刻 t 的弧长与切向速度。头尾速度为 0。"""
|
||||
t = max(0.0, float(t))
|
||||
dist = max(float(dist), 1e-6)
|
||||
v_peak = max(float(v_peak), 1e-4)
|
||||
if t_acc <= 1e-9 and t_cruise <= 1e-9:
|
||||
T = math.pi * dist / (2.0 * v_peak)
|
||||
if t >= T:
|
||||
return dist, 0.0
|
||||
u = math.pi * t / T
|
||||
s = (v_peak * T / math.pi) * (1.0 - math.cos(u))
|
||||
vs = v_peak * math.sin(u)
|
||||
return min(s, dist), max(0.0, vs)
|
||||
t_dec = 2.0 * float(d_dec) / v_peak if v_peak > 1e-9 else 0.0
|
||||
t_end = float(t_acc) + float(t_cruise) + t_dec
|
||||
if t >= t_end:
|
||||
return dist, 0.0
|
||||
if t < t_acc:
|
||||
u = math.pi * t / t_acc
|
||||
s = 0.5 * v_peak * (t - t_acc / math.pi * math.sin(u))
|
||||
vs = 0.5 * v_peak * (1.0 - math.cos(u))
|
||||
return s, vs
|
||||
if t < t_acc + t_cruise:
|
||||
return float(d_acc) + v_peak * (t - t_acc), v_peak
|
||||
td = t - t_acc - t_cruise
|
||||
u = math.pi * td / t_dec if t_dec > 1e-9 else math.pi
|
||||
s0 = float(d_acc) + v_peak * float(t_cruise)
|
||||
s = s0 + 0.5 * v_peak * (td + t_dec / math.pi * math.sin(u))
|
||||
vs = 0.5 * v_peak * (1.0 + math.cos(u))
|
||||
return min(s, dist), max(0.0, vs)
|
||||
|
||||
|
||||
def _circle_3(
|
||||
a: np.ndarray, b: np.ndarray, c: np.ndarray
|
||||
) -> Optional[Tuple[np.ndarray, float]]:
|
||||
ax, ay = float(a[0]), float(a[1])
|
||||
bx, by = float(b[0]), float(b[1])
|
||||
cx, cy = float(c[0]), float(c[1])
|
||||
d = 2.0 * (ax * (by - cy) + bx * (cy - ay) + cx * (ay - by))
|
||||
if abs(d) < 1e-9:
|
||||
return None
|
||||
a2 = ax * ax + ay * ay
|
||||
b2 = bx * bx + by * by
|
||||
c2 = cx * cx + cy * cy
|
||||
ux = (a2 * (by - cy) + b2 * (cy - ay) + c2 * (ay - by)) / d
|
||||
uy = (a2 * (cx - bx) + b2 * (ax - cx) + c2 * (bx - ax)) / d
|
||||
center = np.array([ux, uy], dtype=float)
|
||||
r = float(np.hypot(ax - ux, ay - uy))
|
||||
if r < 1e-4:
|
||||
return None
|
||||
return center, r
|
||||
|
||||
|
||||
def _ccw(a0: float, a1: float) -> float:
|
||||
return (a1 - a0) % (2.0 * math.pi)
|
||||
|
||||
|
||||
def _xy_in_box(xy: np.ndarray, lo: np.ndarray, hi: np.ndarray) -> bool:
|
||||
return bool(
|
||||
float(lo[0]) <= float(xy[0]) <= float(hi[0])
|
||||
and float(lo[1]) <= float(xy[1]) <= float(hi[1])
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class ArcXyMove:
|
||||
"""从 p0 走到 p1 的 XY 圆弧（或退化成直线），弧长上跑梯形速度。"""
|
||||
|
||||
p0: np.ndarray
|
||||
p1: np.ndarray
|
||||
length: float
|
||||
v_cruise: float
|
||||
T: float
|
||||
v_peak: float
|
||||
t_acc: float
|
||||
t_cruise: float
|
||||
a: float
|
||||
t0: float
|
||||
linear: bool
|
||||
center: Optional[np.ndarray] = None
|
||||
radius: float = 0.0
|
||||
ang0: float = 0.0
|
||||
sweep: float = 0.0
|
||||
s_last: float = 0.0
|
||||
_dir: np.ndarray = field(default_factory=lambda: np.array([1.0, 0.0]))
|
||||
|
||||
def sample(self, now: float) -> Tuple[np.ndarray, np.ndarray, bool]:
|
||||
"""返回 (计划 XY, 切向速度, 是否走完)。"""
|
||||
t = max(0.0, float(now) - float(self.t0))
|
||||
s, sdot = trap_sv(
|
||||
t, self.length, self.v_peak, self.t_acc, self.t_cruise, self.a
|
||||
)
|
||||
s = min(float(s), float(self.length))
|
||||
self.s_last = s
|
||||
done = t >= float(self.T) - 1e-4 or s >= float(self.length) - 1e-5
|
||||
if self.linear or self.radius <= 1e-6 or self.center is None:
|
||||
L = max(float(self.length), 1e-6)
|
||||
u = s / L
|
||||
xy = (1.0 - u) * self.p0 + u * self.p1
|
||||
vxy = self._dir * float(sdot)
|
||||
return xy, vxy, done
|
||||
frac = 0.0 if abs(self.sweep) < 1e-9 else s / (self.radius * abs(self.sweep))
|
||||
frac = min(1.0, max(0.0, frac))
|
||||
ang = self.ang0 + self.sweep * frac
|
||||
xy = self.center + self.radius * np.array(
|
||||
[math.cos(ang), math.sin(ang)], dtype=float
|
||||
)
|
||||
tang = np.array([-math.sin(ang), math.cos(ang)], dtype=float)
|
||||
if self.sweep < 0.0:
|
||||
tang = -tang
|
||||
return xy, tang * float(sdot), done
|
||||
|
||||
|
||||
def build_arc_xy_move(
|
||||
p0,
|
||||
p1,
|
||||
*,
|
||||
v: float,
|
||||
a: float,
|
||||
now: float,
|
||||
ws_min=None,
|
||||
ws_max=None,
|
||||
sag_frac: float = D435_ARC_SAGITTA_FRAC,
|
||||
sag_max: float = D435_ARC_SAGITTA_MAX_M,
|
||||
) -> ArcXyMove:
|
||||
"""p0→p1 的圆弧。工作盒装不下就换侧，再不行走直线。速度整段同一档。"""
|
||||
p0 = np.asarray(p0, dtype=float).reshape(2)
|
||||
p1 = np.asarray(p1, dtype=float).reshape(2)
|
||||
chord = p1 - p0
|
||||
L = float(np.hypot(chord[0], chord[1]))
|
||||
direction = (
|
||||
chord / L if L > 1e-6 else np.array([1.0, 0.0], dtype=float)
|
||||
)
|
||||
cruise = max(float(v), 1e-4)
|
||||
acc = max(float(a), 1e-3)
|
||||
|
||||
def _linear() -> ArcXyMove:
|
||||
T, v_peak, t_acc, t_cruise, a_use = trap_params(max(L, 1e-6), cruise, acc)
|
||||
return ArcXyMove(
|
||||
p0=p0.copy(),
|
||||
p1=p1.copy(),
|
||||
length=max(L, 1e-6),
|
||||
v_cruise=cruise,
|
||||
T=T,
|
||||
v_peak=v_peak,
|
||||
t_acc=t_acc,
|
||||
t_cruise=t_cruise,
|
||||
a=a_use,
|
||||
t0=float(now),
|
||||
linear=True,
|
||||
_dir=direction,
|
||||
)
|
||||
|
||||
if L < 0.003:
|
||||
return _linear()
|
||||
|
||||
n = np.array([-direction[1], direction[0]], dtype=float)
|
||||
h = min(float(sag_max), float(sag_frac) * L)
|
||||
mid = 0.5 * (p0 + p1)
|
||||
lo = None if ws_min is None else np.asarray(ws_min, dtype=float).reshape(-1)[:2]
|
||||
hi = None if ws_max is None else np.asarray(ws_max, dtype=float).reshape(-1)[:2]
|
||||
|
||||
def _try(sign: float) -> Optional[ArcXyMove]:
|
||||
apex = mid + n * (h * sign)
|
||||
cir = _circle_3(p0, apex, p1)
|
||||
if cir is None:
|
||||
return None
|
||||
center, r = cir
|
||||
a0 = math.atan2(p0[1] - center[1], p0[0] - center[0])
|
||||
a1 = math.atan2(p1[1] - center[1], p1[0] - center[0])
|
||||
a_via = math.atan2(apex[1] - center[1], apex[0] - center[0])
|
||||
ccw_end = _ccw(a0, a1)
|
||||
ccw_via = _ccw(a0, a_via)
|
||||
if 1e-6 < ccw_via < ccw_end - 1e-6:
|
||||
sweep = ccw_end
|
||||
else:
|
||||
sweep = ccw_end - 2.0 * math.pi
|
||||
if abs(sweep) < 1e-6:
|
||||
return None
|
||||
length = r * abs(sweep)
|
||||
if lo is not None and hi is not None:
|
||||
for k in range(9):
|
||||
frac = k / 8.0
|
||||
ang = a0 + sweep * frac
|
||||
xy = center + r * np.array([math.cos(ang), math.sin(ang)])
|
||||
if not _xy_in_box(xy, lo, hi):
|
||||
return None
|
||||
T, v_peak, t_acc, t_cruise, a_use = trap_params(length, cruise, acc)
|
||||
return ArcXyMove(
|
||||
p0=p0.copy(),
|
||||
p1=p1.copy(),
|
||||
length=length,
|
||||
v_cruise=cruise,
|
||||
T=T,
|
||||
v_peak=v_peak,
|
||||
t_acc=t_acc,
|
||||
t_cruise=t_cruise,
|
||||
a=a_use,
|
||||
t0=float(now),
|
||||
linear=False,
|
||||
center=center,
|
||||
radius=r,
|
||||
ang0=a0,
|
||||
sweep=sweep,
|
||||
)
|
||||
|
||||
for sign in (1.0, -1.0):
|
||||
got = _try(sign)
|
||||
if got is not None:
|
||||
return got
|
||||
return _linear()
|
||||
|
||||
|
||||
def path_interp(waypoints, u: float):
|
||||
"""沿关节路点折线插值。u∈[0,1] → (q, dq/du)。"""
|
||||
nseg = len(waypoints) - 1
|
||||
if nseg <= 0:
|
||||
z = np.zeros_like(waypoints[0])
|
||||
return waypoints[0].copy(), z
|
||||
u = float(min(1.0, max(0.0, u)))
|
||||
x = u * nseg
|
||||
i = min(nseg - 1, int(math.floor(x)))
|
||||
a = x - i
|
||||
q = (1.0 - a) * waypoints[i] + a * waypoints[i + 1]
|
||||
dq_du = (waypoints[i + 1] - waypoints[i]) * nseg
|
||||
return q, dq_du
|
||||
|
||||
|
||||
def translate_hold_ori_waypoints(
|
||||
q0,
|
||||
J,
|
||||
dp,
|
||||
*,
|
||||
damping: float = 0.08,
|
||||
max_dq: float = 0.06,
|
||||
R=None,
|
||||
verticalize: bool = False,
|
||||
):
|
||||
"""冻结当前雅可比，把 ``dp`` 拆成关节路点。
|
||||
|
||||
和二阶段 ``_plan_and_send`` 同一份逆解。``verticalize`` 时顺带收法兰朝下，
|
||||
不许另抄一份 ``_w_down``。返回 ``(路点, 还剩多少米)``。
|
||||
"""
|
||||
from arm_application.geom_ik import dls_translate_hold_ori, dls_twist
|
||||
|
||||
q = np.asarray(q0, dtype=float).copy()
|
||||
remain = np.asarray(dp, dtype=float).reshape(3).copy()
|
||||
dist = float(np.linalg.norm(remain))
|
||||
wps = [q.copy()]
|
||||
inner_tol = min(0.00012, max(0.00005, 0.05 * max(dist, 1e-6)))
|
||||
n_max = int(math.ceil(dist / 0.008)) + 16
|
||||
Jj = np.asarray(J, dtype=float)
|
||||
w_step = np.zeros(3, dtype=float)
|
||||
ang = 0.0
|
||||
if verticalize:
|
||||
if R is None:
|
||||
raise ValueError("verticalize 必须传入当前法兰 R，不许假装已经朝下")
|
||||
w_step, ang = flange_down_w(R)
|
||||
for _ in range(n_max):
|
||||
if float(np.linalg.norm(remain)) < inner_tol and (
|
||||
(not verticalize) or ang < 0.006
|
||||
):
|
||||
break
|
||||
nseg = max(1, int(math.ceil(float(np.linalg.norm(remain)) / 0.012)))
|
||||
step = remain / nseg
|
||||
if verticalize and ang > 0.008:
|
||||
qdot = dls_twist(Jj, step / 0.12, w_base=w_step, damping=damping, max_qdot=0.45)
|
||||
dq = np.clip(qdot * 0.12, -float(max_dq), float(max_dq))
|
||||
else:
|
||||
dq = dls_translate_hold_ori(Jj, step, damping=damping, max_dq=max_dq)
|
||||
if float(np.max(np.abs(dq))) < 1e-5:
|
||||
break
|
||||
q = q + dq
|
||||
wps.append(q.copy())
|
||||
remain = remain - (Jj[:3, :] @ dq)
|
||||
ang = max(0.0, ang - float(np.linalg.norm(w_step)) * 0.12)
|
||||
return wps, float(np.linalg.norm(remain))
|
||||
|
||||
|
||||
def trap_joint_schedule(
|
||||
waypoints,
|
||||
dist: float,
|
||||
v: float,
|
||||
a: float,
|
||||
*,
|
||||
t0: float = 0.06,
|
||||
dt: float = 0.04,
|
||||
t_hold: float = 0.25,
|
||||
):
|
||||
"""路点 + 梯形速度 → ``[(q, qdot, t), ...]``。头尾速度为 0，时间严格递增。
|
||||
|
||||
一次下发整段。中途再发下一条会把这条掐掉，臂就重新加速 —— 那就是抖。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
T_move, v_peak, t_acc, t_cruise, a_use = trap_params(dist, v, a)
|
||||
n_samp = max(8, int(math.ceil(T_move / max(float(dt), 1e-3))))
|
||||
zeros = np.zeros_like(waypoints[0])
|
||||
out = []
|
||||
last_t = -1.0
|
||||
for k in range(n_samp + 1):
|
||||
t_local = T_move * k / n_samp
|
||||
s, vs = trap_sv(t_local, dist, v_peak, t_acc, t_cruise, a_use)
|
||||
u = s / dist
|
||||
qi, dq_du = path_interp(waypoints, u)
|
||||
qdot = dq_du * (vs / dist) if k < n_samp else zeros
|
||||
if k >= n_samp:
|
||||
qi = waypoints[-1]
|
||||
qdot = zeros
|
||||
t = float(t0) + t_local
|
||||
if t <= last_t:
|
||||
t = last_t + 0.02
|
||||
out.append((np.asarray(qi, dtype=float).copy(), np.asarray(qdot, dtype=float).copy(), t))
|
||||
last_t = t
|
||||
hold = max(0.12, float(t_hold))
|
||||
for frac in (0.40, 0.70, 1.00):
|
||||
t = float(t0) + T_move + hold * frac
|
||||
if t < last_t + 0.05:
|
||||
t = last_t + 0.05
|
||||
out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
|
||||
last_t = t
|
||||
return out, T_move, v_peak
|
||||
|
||||
|
||||
def smooth_joint_schedule(
|
||||
waypoints,
|
||||
dist: float,
|
||||
v: float,
|
||||
*,
|
||||
t0: float = 0.06,
|
||||
dt: float = 0.04,
|
||||
t_hold: float = 0.25,
|
||||
d_acc: float = D435_FULL_ACC_M,
|
||||
d_dec: float = D435_FULL_DEC_M,
|
||||
):
|
||||
"""路点 + 余弦加减速 → ``[(q, qdot, t), ...]``。头尾速度为 0。
|
||||
|
||||
D435 整段一次下发用这一份。二阶段短跳仍走 ``trap_joint_schedule``。
|
||||
"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
T_move, v_peak, t_acc, t_cruise, acc_m, dec_m = smooth_path_params(
|
||||
dist, v, d_acc=d_acc, d_dec=d_dec
|
||||
)
|
||||
n_samp = max(12, int(math.ceil(T_move / max(float(dt), 1e-3))))
|
||||
zeros = np.zeros_like(waypoints[0])
|
||||
out = []
|
||||
last_t = -1.0
|
||||
for k in range(n_samp + 1):
|
||||
t_local = T_move * k / n_samp
|
||||
s, vs = smooth_path_sv(
|
||||
t_local, dist, v_peak, t_acc, t_cruise, acc_m, dec_m
|
||||
)
|
||||
u = min(1.0, s / dist)
|
||||
qi, dq_du = path_interp(waypoints, u)
|
||||
qdot = dq_du * (vs / dist) if k < n_samp else zeros
|
||||
if k >= n_samp:
|
||||
qi = waypoints[-1]
|
||||
qdot = zeros
|
||||
t = float(t0) + t_local
|
||||
if t <= last_t:
|
||||
t = last_t + 0.02
|
||||
out.append(
|
||||
(
|
||||
np.asarray(qi, dtype=float).copy(),
|
||||
np.asarray(qdot, dtype=float).copy(),
|
||||
t,
|
||||
)
|
||||
)
|
||||
last_t = t
|
||||
hold = max(0.12, float(t_hold))
|
||||
for frac in (0.40, 0.70, 1.00):
|
||||
t = float(t0) + T_move + hold * frac
|
||||
if t < last_t + 0.05:
|
||||
t = last_t + 0.05
|
||||
out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
|
||||
last_t = t
|
||||
return out, T_move, v_peak
|
||||
@@ -31,7 +31,7 @@ from rclpy.qos import (
|
||||
qos_profile_sensor_data,
|
||||
)
|
||||
from sensor_msgs.msg import CameraInfo, Image
|
||||
from std_msgs.msg import Float32, Float32MultiArray
|
||||
from std_msgs.msg import Bool, Float32, Float32MultiArray
|
||||
from tf2_ros import TransformBroadcaster
|
||||
|
||||
from arm_application.box_tracker import BoxTracker
|
||||
@@ -46,11 +46,16 @@ from arm_application.handeye_realsense import (
|
||||
resolve_realsense_topics,
|
||||
)
|
||||
from arm_application.overhead_oneshot import (
|
||||
D405_MISS_BEFORE_D435_S,
|
||||
ONESHOT_ACCEPT_TIMEOUT_S,
|
||||
best_det_conf,
|
||||
depth_raw_to_m,
|
||||
oneshot_k0_base,
|
||||
oneshot_shot_timed_out,
|
||||
oneshot_should_submit,
|
||||
select_oneshot_det,
|
||||
wrist_needs_handoff,
|
||||
z_locked_qos,
|
||||
)
|
||||
from arm_application.kalman_filter import KalmanFilter3D
|
||||
from arm_application.plane_feature import (
|
||||
@@ -161,6 +166,7 @@ class PerceptionNode(TolerantNode):
|
||||
# ---- D405 主跟踪 + D435 一次性（键必须从 site 接入，缺了启动即红）----
|
||||
self.declare_parameter("wrist_handoff_conf", -1.0)
|
||||
self.declare_parameter("overhead_oneshot_enable", False)
|
||||
self.declare_parameter("d435_move_only", False)
|
||||
self.declare_parameter("overhead_min_depth_m", -1.0)
|
||||
self.declare_parameter("overhead_max_depth_m", -1.0)
|
||||
self.declare_parameter("overhead_oneshot_conf", -1.0)
|
||||
@@ -210,9 +216,10 @@ class PerceptionNode(TolerantNode):
|
||||
# 跟踪点三道守卫（单一实现见 feature_guard.py）
|
||||
self.keypoint_border_px = float(self.get_parameter("keypoint_border_px").value)
|
||||
self.track_max_step_m = float(self.get_parameter("track_max_step_m").value)
|
||||
self.reacquire_far_px = float(self.get_parameter("reacquire_far_px").value)
|
||||
self._slew = TrackSlewGuard(self.track_max_step_m)
|
||||
self._far = FarReacquireConfirm(
|
||||
float(self.get_parameter("reacquire_far_px").value),
|
||||
self.reacquire_far_px,
|
||||
int(self.get_parameter("reacquire_confirm_frames").value),
|
||||
)
|
||||
self.plane_inlier_m = float(self.get_parameter("plane_inlier_m").value)
|
||||
@@ -220,6 +227,7 @@ class PerceptionNode(TolerantNode):
|
||||
self.overhead_oneshot_enable = bool(
|
||||
self.get_parameter("overhead_oneshot_enable").value
|
||||
)
|
||||
self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
|
||||
self._T_oh: Optional[np.ndarray] = None
|
||||
self._oh_image: Optional[Image] = None
|
||||
self._oh_depth: Optional[Image] = None
|
||||
@@ -230,6 +238,11 @@ class PerceptionNode(TolerantNode):
|
||||
self._oneshot_t = -1.0e9
|
||||
self._need_oneshot = False
|
||||
self._oneshot_inflight = False
|
||||
self._oneshot_shot = False
|
||||
self._oneshot_shot_t = -1.0e9
|
||||
self._low_since: Optional[float] = None
|
||||
self._d435_commanding = False
|
||||
self._z_locked = False
|
||||
self._oneshot_img: Optional[Image] = None
|
||||
self._oneshot_depth: Optional[Image] = None
|
||||
if self.overhead_oneshot_enable:
|
||||
@@ -441,10 +454,28 @@ class PerceptionNode(TolerantNode):
|
||||
CameraInfo, oh_info, self._on_oh_info, image_qos,
|
||||
callback_group=self._io_cbg,
|
||||
)
|
||||
self.create_subscription(
|
||||
Bool, "visual_servo/z_locked", self._on_z_locked, z_locked_qos(),
|
||||
callback_group=self._io_cbg,
|
||||
)
|
||||
self.create_subscription(
|
||||
Bool,
|
||||
"visual_servo/d435_commanding",
|
||||
self._on_d435_commanding,
|
||||
z_locked_qos(),
|
||||
callback_group=self._io_cbg,
|
||||
)
|
||||
extra = (
|
||||
"；d435_move_only：不等 D405 丢目标，0.5s 后必拍一张"
|
||||
if self.d435_move_only
|
||||
else ""
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"D435 一次性 image={oh_img} depth={oh_dep} "
|
||||
f"D435 待命 image={oh_img} depth={oh_dep} "
|
||||
f"handoff_conf={self.wrist_handoff_conf:.2f} "
|
||||
f"看不到满 {D405_MISS_BEFORE_D435_S:.1f}s 才拍一张 "
|
||||
f"T={self.get_parameter('overhead_handeye_file').value}"
|
||||
f"{extra}"
|
||||
)
|
||||
|
||||
point_qos = QoSProfile(
|
||||
@@ -865,9 +896,52 @@ class PerceptionNode(TolerantNode):
|
||||
self._publish_wrist_conf(best_conf)
|
||||
handoff = bool(
|
||||
self.overhead_oneshot_enable
|
||||
and wrist_needs_handoff(best_conf, self.wrist_handoff_conf)
|
||||
and (
|
||||
self.d435_move_only
|
||||
or wrist_needs_handoff(best_conf, self.wrist_handoff_conf)
|
||||
)
|
||||
)
|
||||
self._need_oneshot = handoff
|
||||
now_m = time.monotonic()
|
||||
if self._d435_commanding:
|
||||
# 臂还在赶 D435 那个点。漏检计时清掉，走到并停稳之后再重新计。
|
||||
self._low_since = None
|
||||
self._need_oneshot = False
|
||||
elif not handoff:
|
||||
self._low_since = None
|
||||
self._oneshot_shot = False
|
||||
self._need_oneshot = False
|
||||
else:
|
||||
if oneshot_shot_timed_out(
|
||||
shot=self._oneshot_shot,
|
||||
commanding=self._d435_commanding,
|
||||
age_s=now_m - float(self._oneshot_shot_t),
|
||||
timeout_s=ONESHOT_ACCEPT_TIMEOUT_S,
|
||||
):
|
||||
self.get_logger().warn(
|
||||
"D435 拍了但伺服没接住（工作盒拒收等），允许再拍"
|
||||
)
|
||||
self._oneshot_shot = False
|
||||
self._low_since = None
|
||||
if self._low_since is None:
|
||||
self._low_since = now_m
|
||||
low_for = now_m - self._low_since
|
||||
self._need_oneshot = oneshot_should_submit(
|
||||
handoff=True,
|
||||
low_for_s=low_for,
|
||||
already_shot=self._oneshot_shot,
|
||||
wait_s=D405_MISS_BEFORE_D435_S,
|
||||
)
|
||||
if self._oneshot_shot:
|
||||
self.get_logger().info(
|
||||
"D435 这一轮已经拍过一张，等这一段走完，不再连拍",
|
||||
throttle_duration_sec=2.0,
|
||||
)
|
||||
elif not self._need_oneshot:
|
||||
self.get_logger().info(
|
||||
f"D405 已连续 {low_for:.2f}s 没有高置信度目标，"
|
||||
f"未满 {D405_MISS_BEFORE_D435_S:.1f}s，D435 继续待命",
|
||||
throttle_duration_sec=0.5,
|
||||
)
|
||||
now = time.monotonic()
|
||||
pred_at_img = self._box_trk.predict(t_img, self._img_wh())
|
||||
with self._lock:
|
||||
@@ -1326,6 +1400,21 @@ class PerceptionNode(TolerantNode):
|
||||
msg.data = float("nan") if best_conf is None else float(best_conf)
|
||||
self._wrist_conf_pub.publish(msg)
|
||||
|
||||
def _on_z_locked(self, msg: Bool) -> None:
|
||||
"""锁高状态只记着。拍不拍 D435 不看它，看 D405 连续漏检。"""
|
||||
self._z_locked = bool(msg.data)
|
||||
|
||||
def _on_d435_commanding(self, msg: Bool) -> None:
|
||||
"""伺服还在执行 D435 那一个点时，不再提交下一张。
|
||||
|
||||
这一段结束（到位并停稳）后清掉「已拍过」，漏检重新计 0.5 s。
|
||||
"""
|
||||
commanding = bool(msg.data)
|
||||
if self._d435_commanding and not commanding:
|
||||
self._oneshot_shot = False
|
||||
self._low_since = None
|
||||
self._d435_commanding = commanding
|
||||
|
||||
def _publish_overhead_k0(self, p_base: np.ndarray, is_k0: bool) -> None:
|
||||
pt = PointStamped()
|
||||
pt.header.stamp = self.get_clock().now().to_msg()
|
||||
@@ -1447,6 +1536,9 @@ class PerceptionNode(TolerantNode):
|
||||
)
|
||||
return
|
||||
p_base, is_k0 = got
|
||||
self._oneshot_shot = True
|
||||
self._oneshot_shot_t = now
|
||||
self._need_oneshot = False
|
||||
self._publish_overhead_k0(p_base, is_k0)
|
||||
self.get_logger().info(
|
||||
f"D435 一次性定位 {'k0' if is_k0 else '框心（顶视看不见 k0）'} "
|
||||
|
||||
@@ -26,16 +26,33 @@ from std_msgs.msg import Bool, String
|
||||
from arm_application.stage3_search_node import Stage3Search
|
||||
|
||||
|
||||
#: 一阶段跟踪高相对二阶段悬停高的**抬升量** [m]。
|
||||
#: 悬停时规尖离面不够，跟踪高再抬的量 [m]。
|
||||
#:
|
||||
#: 为什么要抬：跟踪时通止规已经夹在爪上，规尖比相机更靠下；悬停高是"相机拍清
|
||||
#: 孔"的高度，直接在那儿跟踪会让规尖刮到工件。抬 5cm 让它离开工件，二阶段再
|
||||
#: 小幅下降（只需 5cm 以内的行程）。
|
||||
#: 只在「悬停高上规尖离工件面 < :data:`TRACK_LOCK_GAP_M`」时加上。仿真悬停按
|
||||
#: 工具几何只留约 2mm，不抬就会在跟踪时刮到工件。真机 89 悬停 0.320 时规尖
|
||||
#: 已经在面上方约 75mm，再抬 50mm 会让对孔前先改一次高度（0.37→0.32）。
|
||||
#:
|
||||
#: ⚠️ 抽成常量是因为它不只被流程用：``scripts/check_sim_scene.py`` 要用它核对
|
||||
#: "跟踪高有没有超出工作盒上界"（仿真里悬停 0.632 + 0.05 = 0.682 > 真机那套
|
||||
#: 0.65 上界，就是被这条查出来的）。两处各写一个字面量迟早漂移。
|
||||
TRACK_CLEARANCE_M = 0.05
|
||||
#: 悬停时规尖离面已经不少于这个值 [m]，一阶段就锁在悬停高，对孔完成后再下降。
|
||||
TRACK_LOCK_GAP_M = 0.040
|
||||
|
||||
|
||||
def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
|
||||
"""一阶段锁高。
|
||||
|
||||
悬停时规尖离面已经 ≥ ``TRACK_LOCK_GAP_M``：锁在悬停高，跟踪和对孔都不改 Z，
|
||||
对孔完成后再下降。离面不够（仿真悬停只留 2mm）：再抬 ``TRACK_CLEARANCE_M``，
|
||||
否则跟踪会刮到工件。工具长或工件面缺失时抬升 —— 缺数时锁在悬停高等于没检查。
|
||||
"""
|
||||
tool = float(tool_len_m)
|
||||
surf = float(surface_z_m)
|
||||
hover = float(hover_z)
|
||||
if tool > 0.0 and surf > 0.0 and (hover - tool - surf) >= TRACK_LOCK_GAP_M:
|
||||
return hover
|
||||
return hover + TRACK_CLEARANCE_M
|
||||
|
||||
|
||||
def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
|
||||
@@ -79,6 +96,7 @@ class AssemblyPipeline(Stage3Search):
|
||||
self.declare_parameter("skip_stage1", False)
|
||||
self.declare_parameter("skip_stage2", False)
|
||||
self.declare_parameter("skip_stage3", False) # 默认搜孔+轻压；拧入靠 --depth
|
||||
self.declare_parameter("d435_move_only", False)
|
||||
self.declare_parameter("stage1_timeout_sec", 150.0)
|
||||
self.linger3 = 1.2 # 串联结束短保持；单独跑 stage3 仍用 site linger_sec
|
||||
|
||||
@@ -113,9 +131,11 @@ class AssemblyPipeline(Stage3Search):
|
||||
skip1 = bool(self.get_parameter("skip_stage1").value)
|
||||
skip2 = bool(self.get_parameter("skip_stage2").value)
|
||||
skip3 = bool(self.get_parameter("skip_stage3").value)
|
||||
self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
|
||||
self.get_logger().info(
|
||||
f"pipeline arm_ee={self.ee_frame} hole={self.target_hole} "
|
||||
f"skip1={skip1} skip2={skip2} skip3={skip3} insert={self.insert_depth_mm:.1f}mm"
|
||||
f"skip1={skip1} skip2={skip2} skip3={skip3} "
|
||||
f"d435_only={self.d435_move_only} insert={self.insert_depth_mm:.1f}mm"
|
||||
)
|
||||
|
||||
def _state(self, s: str) -> None:
|
||||
@@ -163,8 +183,12 @@ class AssemblyPipeline(Stage3Search):
|
||||
self._kill_old_stage1()
|
||||
|
||||
def _track_z(self) -> float:
|
||||
"""跟踪高度 = 孔悬停高 + :data:`TRACK_CLEARANCE_M`，通止规离开工件。"""
|
||||
return float(self.hover_z) + TRACK_CLEARANCE_M
|
||||
"""一阶段锁高：见 :func:`track_lock_z`。对孔完成前不再改这个高度。"""
|
||||
return track_lock_z(
|
||||
float(self.hover_z),
|
||||
float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
|
||||
float(getattr(self, "surface_z_m", 0.0) or 0.0),
|
||||
)
|
||||
|
||||
def _ensure_clearance(self) -> None:
|
||||
"""上一轮轻压可能还在孔里：先纯 Z 抬到跟踪高，禁止 0.23↔0.35 来回。"""
|
||||
@@ -177,7 +201,7 @@ class AssemblyPipeline(Stage3Search):
|
||||
return
|
||||
self._state("pre_lift")
|
||||
self.get_logger().info(
|
||||
f"先离开工件/孔口：Z {z:.3f} → {tz:.3f}m（跟踪高=hover+50mm）"
|
||||
f"先到锁高：Z {z:.3f} → {tz:.3f}m（跟踪与对孔都停在这个高度，对完孔再下降）"
|
||||
)
|
||||
_, q, Tee = self._goto(
|
||||
q, Tee, np.array([float(Tee[0, 3]), float(Tee[1, 3]), tz]),
|
||||
@@ -206,10 +230,16 @@ class AssemblyPipeline(Stage3Search):
|
||||
"publish_handeye_tf:=false",
|
||||
f"ee_z_m:={lock_z:.3f}",
|
||||
f"use_sim_time:={str(use_sim).lower()}",
|
||||
f"d435_move_only:={str(self.d435_move_only).lower()}",
|
||||
]
|
||||
self.get_logger().info(
|
||||
f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
|
||||
)
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
f"启动 D435 位移测试（锁高 {lock_z:.3f}m，到位后不交 D405、不进二阶段）"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
|
||||
)
|
||||
self._s1 = subprocess.Popen(
|
||||
cmd, stdout=logf, stderr=subprocess.STDOUT, start_new_session=True,
|
||||
)
|
||||
@@ -229,7 +259,12 @@ class AssemblyPipeline(Stage3Search):
|
||||
stage1_timeout_reason(self._point_msgs, timeout, log_path)
|
||||
)
|
||||
return 1
|
||||
self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
"D435 位移测试结束 — 停跟踪、钉住当前关节，跳过二阶段"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
|
||||
self._stop_stage1()
|
||||
time.sleep(0.35)
|
||||
q = self._q()
|
||||
@@ -241,6 +276,9 @@ class AssemblyPipeline(Stage3Search):
|
||||
skip1 = bool(self.get_parameter("skip_stage1").value)
|
||||
skip2 = bool(self.get_parameter("skip_stage2").value)
|
||||
skip3 = bool(self.get_parameter("skip_stage3").value)
|
||||
if self.d435_move_only:
|
||||
skip2 = True
|
||||
skip3 = True
|
||||
if not self._cli.wait_for_server(timeout_sec=5.0):
|
||||
self.get_logger().error("JTC action 不在 —— 先开 T1: run_real.sh --arm 89")
|
||||
return 2
|
||||
@@ -268,17 +306,30 @@ class AssemblyPipeline(Stage3Search):
|
||||
return code
|
||||
self.get_logger().info(
|
||||
f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
|
||||
+ (" — 跳过搜孔，保持悬停" if skip3 else " — 立刻下压搜孔并轻压（不拧入）")
|
||||
+ (
|
||||
" — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
|
||||
if skip3
|
||||
else " — 立刻下压搜孔并轻压（不拧入）"
|
||||
)
|
||||
)
|
||||
else:
|
||||
self._state("skip_stage2")
|
||||
|
||||
if skip3:
|
||||
self._state("skip_stage3")
|
||||
self.get_logger().info("跳过搜孔 —— 臂保持在孔上方")
|
||||
if self.d435_move_only:
|
||||
self.get_logger().info(
|
||||
"D435 位移测试结束 —— 臂停在当前高度，不交 D405、不对孔；"
|
||||
"15s 后本进程退出，臂保持不动"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
"跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；"
|
||||
"15s 后本进程退出，臂保持不动"
|
||||
)
|
||||
if final:
|
||||
self._publish_result(final)
|
||||
self._linger_hold(final if final else {"ok": True}, seconds=0.8)
|
||||
self._linger_hold(final if final else {"ok": True}, seconds=15.0)
|
||||
payload = {"ok": True, "stage": "pipeline", "exit": 0, "skipped": "stage3"}
|
||||
try:
|
||||
payload["stage2"] = json.loads(
|
||||
|
||||
@@ -24,9 +24,17 @@ import rclpy
|
||||
from geometry_msgs.msg import PointStamped, Twist
|
||||
from std_msgs.msg import Bool, Float32
|
||||
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
|
||||
from rclpy.action import ActionClient
|
||||
from rclpy.executors import MultiThreadedExecutor
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, qos_profile_system_default
|
||||
from control_msgs.action import FollowJointTrajectory
|
||||
from rclpy.qos import (
|
||||
DurabilityPolicy,
|
||||
HistoryPolicy,
|
||||
QoSProfile,
|
||||
ReliabilityPolicy,
|
||||
qos_profile_system_default,
|
||||
)
|
||||
from sensor_msgs.msg import JointState
|
||||
from tf2_ros import Buffer, TransformListener
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
@@ -39,7 +47,26 @@ from arm_application.geom_ik import (
|
||||
quat_xyzw_to_R,
|
||||
)
|
||||
from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T
|
||||
from arm_application.overhead_oneshot import StillLock, wrist_needs_handoff
|
||||
from arm_application.overhead_oneshot import (
|
||||
D435_NEAR_CRUISE_MPS,
|
||||
D435_SETTLE_S,
|
||||
StillLock,
|
||||
at_lock_height,
|
||||
d405_commands_arm,
|
||||
flange_tilt_rad,
|
||||
hold_translation_vz,
|
||||
hybrid_cruise_vxy,
|
||||
lock_z_velocity,
|
||||
oneshot_concat_hold_ori,
|
||||
oneshot_full_cruise_mps,
|
||||
oneshot_full_segments,
|
||||
oneshot_hop_limits,
|
||||
oneshot_may_send_another,
|
||||
oneshot_needs_verticalize,
|
||||
wrist_needs_handoff,
|
||||
z_locked_qos,
|
||||
)
|
||||
from arm_application.path_profile import smooth_joint_schedule, translate_hold_ori_waypoints
|
||||
from arm_application.pid_controller import PidController
|
||||
from arm_application.kalman_filter import KalmanFilter3D
|
||||
from arm_application.feature_jump_guard import (
|
||||
@@ -83,6 +110,9 @@ class ServoNode(TolerantNode):
|
||||
self.declare_parameter("ee_frame", ee_frame_default())
|
||||
self.declare_parameter("camera_frame", "d405_color_optical_frame")
|
||||
self.declare_parameter("traj_topic", "rizon_arm_controller/joint_trajectory")
|
||||
self.declare_parameter(
|
||||
"action_name", "rizon_arm_controller/follow_joint_trajectory"
|
||||
)
|
||||
self.declare_parameter("point_topic", "visual_servo/filtered_point")
|
||||
# 默认值只作兜底：真机运行一定由 launch 传入 site.yaml 的 paths.handeye_file。
|
||||
# 指向 /tmp 是历史遗留（标定临时产物），会让人误以为"标定结果在那里"。
|
||||
@@ -187,6 +217,8 @@ class ServoNode(TolerantNode):
|
||||
self.declare_parameter("lock_still_move_m", -1.0)
|
||||
self.declare_parameter("wrist_handoff_conf", -1.0)
|
||||
self.declare_parameter("overhead_hold_s", -1.0)
|
||||
# 只测 D435 位移：到位后不交 D405、不靠 D405 静止去锁二阶段。
|
||||
self.declare_parameter("d435_move_only", False)
|
||||
self.declare_parameter("overhead_k0_topic", "visual_servo/overhead_k0")
|
||||
self.declare_parameter("wrist_conf_topic", "visual_servo/wrist_conf")
|
||||
|
||||
@@ -271,6 +303,7 @@ class ServoNode(TolerantNode):
|
||||
self.lock_still_move_m = float(self.get_parameter("lock_still_move_m").value)
|
||||
self.wrist_handoff_conf = float(self.get_parameter("wrist_handoff_conf").value)
|
||||
self.overhead_hold_s = float(self.get_parameter("overhead_hold_s").value)
|
||||
self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
|
||||
self._hybrid = (
|
||||
self.lock_still_sec >= 0.0
|
||||
and self.lock_still_move_m >= 0.0
|
||||
@@ -281,6 +314,26 @@ class ServoNode(TolerantNode):
|
||||
still_sec=max(0.0, self.lock_still_sec),
|
||||
still_move_m=max(0.0, self.lock_still_move_m),
|
||||
)
|
||||
# D435 那一段的「到位并停稳」，和进二阶段用同一套静止判据。
|
||||
self._arrive = StillLock(
|
||||
still_sec=max(0.0, self.lock_still_sec),
|
||||
still_move_m=max(0.0, self.lock_still_move_m),
|
||||
)
|
||||
self._d435_settled = False
|
||||
self._d435_exec_until = 0.0
|
||||
self._d435_pinned = False
|
||||
self._d435_sent = False
|
||||
self._d435_last_dist = None
|
||||
self._d435_give_up = False
|
||||
self._d435_plan_lock = threading.Lock()
|
||||
self._d435_inflight = False
|
||||
self._d435_settle_until = 0.0
|
||||
self._d435_last_z_err = None
|
||||
self._d435_z_stall = 0
|
||||
self._d435_remeasure_log = False
|
||||
self._d435_vert_done = False
|
||||
self._d435_full_done = False
|
||||
self._d435_fix_done = False
|
||||
self._track_source = "wrist" # wrist | overhead
|
||||
self._wrist_conf: Optional[float] = None
|
||||
self._wrist_conf_t = -1.0
|
||||
@@ -337,6 +390,11 @@ class ServoNode(TolerantNode):
|
||||
f"optical_vs_flangeZ={self._cam_tilt_deg:.1f}deg "
|
||||
f"— 悬停使用这个 T，而不是用 TF 查询"
|
||||
)
|
||||
if self.d435_move_only:
|
||||
self.get_logger().warn(
|
||||
"d435_move_only：只跑 D435 单次位移。到位停稳后钉住，"
|
||||
"不交 D405；locked 只用来结束一阶段，不进二阶段"
|
||||
)
|
||||
|
||||
self.pid_x = PidController(
|
||||
float(self.get_parameter("kp_xy").value),
|
||||
@@ -391,6 +449,7 @@ class ServoNode(TolerantNode):
|
||||
self._jump_gate = FeatureJumpGate(self.predict_jump_m, self.jump_confirm_frames)
|
||||
# 只有当法兰 Z 已经落到锁定平面上（深度新鲜）才开启 XY 悬停。
|
||||
self._xy_armed = bool(self.ee_z_m <= 0.0)
|
||||
self._z_hold: Optional[float] = None
|
||||
# Seek（搜索）状态：在锁定高度且没有目标时扫描工件。
|
||||
self._seek_t0 = 0.0
|
||||
self._seek_theta = 0.0
|
||||
@@ -447,11 +506,30 @@ class ServoNode(TolerantNode):
|
||||
)
|
||||
# 一阶段锁定信号（串联编排器用它判断可以进入二阶段）
|
||||
self._lock_pub = self.create_publisher(Bool, "visual_servo/locked", 1)
|
||||
# TRANSIENT_LOCAL：感知节点比伺服晚起来，默认 QoS 会丢掉启动时那一拍
|
||||
# True，感知就永远停在「还没到锁高」、D435 不拍。两边必须同一份 QoS。
|
||||
self._z_lock_pub = self.create_publisher(
|
||||
Bool, "visual_servo/z_locked", z_locked_qos()
|
||||
)
|
||||
# 感知用它挡住「平移半路再拍一张」。晚启动也能拿到当前值。
|
||||
self._d435_cmd_pub = self.create_publisher(
|
||||
Bool, "visual_servo/d435_commanding", z_locked_qos()
|
||||
)
|
||||
self._d435_cmd_sent: Optional[bool] = None
|
||||
self._z_locked_sent: Optional[bool] = None
|
||||
self._z_lock_pub_mono = 0.0
|
||||
self._lock_since: Optional[float] = None
|
||||
self._locked = False
|
||||
self._lock_pub_t = 0.0
|
||||
traj_topic = str(self.get_parameter("traj_topic").value)
|
||||
self._traj_pub = self.create_publisher(JointTrajectory, traj_topic, cmd_qos)
|
||||
self._jtc_cbg = ReentrantCallbackGroup()
|
||||
self._jtc = ActionClient(
|
||||
self,
|
||||
FollowJointTrajectory,
|
||||
str(self.get_parameter("action_name").value),
|
||||
callback_group=self._jtc_cbg,
|
||||
)
|
||||
|
||||
self._tf = Buffer()
|
||||
TransformListener(self._tf, self, spin_thread=True)
|
||||
@@ -477,6 +555,36 @@ class ServoNode(TolerantNode):
|
||||
f"hold_xy={int(self.ee_hold_xy)} down={int(self.ee_vertical_down)} "
|
||||
f"XY_after_lock_z±{self.approach_xy_gate_m*1000:.0f}mm"
|
||||
)
|
||||
self._publish_z_locked(self._xy_armed)
|
||||
self._publish_d435_commanding(False)
|
||||
|
||||
def _publish_z_locked(self, locked: bool) -> None:
|
||||
"""法兰是否在锁高带里。只记日志，不拦 D435。
|
||||
|
||||
状态没变也每 0.5 s 再发一次：只发一次的话，晚订阅的感知收不到。
|
||||
"""
|
||||
locked = bool(locked)
|
||||
now = time.monotonic()
|
||||
changed = self._z_locked_sent is not locked
|
||||
if (not changed) and (now - self._z_lock_pub_mono) < 0.5:
|
||||
return
|
||||
self._z_locked_sent = locked
|
||||
self._z_lock_pub_mono = now
|
||||
self._z_lock_pub.publish(Bool(data=locked))
|
||||
if not changed:
|
||||
return
|
||||
if locked:
|
||||
self.get_logger().info("锁高已到位")
|
||||
else:
|
||||
self.get_logger().info("离开锁高带 —— 平移仍保持当前高度，不改去追锁高")
|
||||
|
||||
def _publish_d435_commanding(self, commanding: bool) -> None:
|
||||
"""正在用 D435 那一个点赶路。感知看到 True 就不再拍第二张。"""
|
||||
commanding = bool(commanding)
|
||||
if self._d435_cmd_sent is commanding:
|
||||
return
|
||||
self._d435_cmd_sent = commanding
|
||||
self._d435_cmd_pub.publish(Bool(data=commanding))
|
||||
|
||||
def _on_js(self, msg: JointState) -> None:
|
||||
self._last_js = msg
|
||||
@@ -504,8 +612,19 @@ class ServoNode(TolerantNode):
|
||||
if not np.all(np.isfinite(p_base)):
|
||||
return
|
||||
if not self._feat_workspace_ok(p_base):
|
||||
m = float(self.feat_ws_margin_m)
|
||||
self.get_logger().warn(
|
||||
f"D435 一次性点 ({p_base[0]:.3f},{p_base[1]:.3f}) 在工作盒外 —— 丢弃",
|
||||
f"D435 一次性点 ({p_base[0]:.3f},{p_base[1]:.3f}) 在工作盒外 "
|
||||
f"（x[{self.ws_min[0]:.2f},{self.ws_max[0]:.2f}] "
|
||||
f"y[{self.ws_min[1]:.2f},{self.ws_max[1]:.2f}] ±{m:.2f}）"
|
||||
"—— 丢弃，不发轨迹。这不是起步加速度。"
|
||||
)
|
||||
return
|
||||
with self._lock:
|
||||
busy = self._track_source == "overhead" and not self._d435_settled
|
||||
if busy:
|
||||
self.get_logger().info(
|
||||
"D435 这一段还没走完，不再换目标",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
return
|
||||
@@ -520,15 +639,31 @@ class ServoNode(TolerantNode):
|
||||
self._have_target = True
|
||||
self._target_lost_flag = False
|
||||
self._track_source = "overhead"
|
||||
self._d435_settled = False
|
||||
self._overhead_t = now_m
|
||||
self._last_seen_time = now_m
|
||||
self._stamp_sec = now_sec
|
||||
self._seek_t0 = 0.0
|
||||
self._seek_warned = False
|
||||
self._still.reset()
|
||||
self._arrive.reset()
|
||||
self._d435_exec_until = 0.0
|
||||
self._d435_pinned = False
|
||||
self._d435_sent = False
|
||||
self._d435_last_dist = None
|
||||
self._d435_give_up = False
|
||||
self._d435_inflight = False
|
||||
self._d435_settle_until = 0.0
|
||||
self._d435_last_z_err = None
|
||||
self._d435_z_stall = 0
|
||||
self._d435_remeasure_log = False
|
||||
self._d435_vert_done = False
|
||||
self._d435_full_done = False
|
||||
self._d435_fix_done = False
|
||||
self._publish_d435_commanding(True)
|
||||
self.get_logger().info(
|
||||
f"D435 一次性设定 feat_base=({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f}) "
|
||||
f"—— 迁过去等 D405 重新看见"
|
||||
f"—— 走到这个点并停稳之前，不下发 D405 的点"
|
||||
)
|
||||
|
||||
def _on_point(self, msg: PointStamped) -> None:
|
||||
@@ -541,16 +676,26 @@ class ServoNode(TolerantNode):
|
||||
# 已经收到腕部 conf 且低于门槛：这条 D405 点不能当跟踪源。
|
||||
# conf 还是 None = 感知还没报到，不要把开场几帧也丢掉。
|
||||
return
|
||||
with self._lock:
|
||||
using_d435 = self._track_source == "overhead"
|
||||
settled = self._d435_settled
|
||||
if self._hybrid and not d405_commands_arm(
|
||||
using_d435=using_d435,
|
||||
arrived=settled,
|
||||
settled=settled,
|
||||
move_only=self.d435_move_only,
|
||||
):
|
||||
# 推理服务不关。这一帧的点不下发给臂，目标仍是 D435 那一个点。
|
||||
self.get_logger().info(
|
||||
"D435 位置还没到并且停稳，D405 这帧不下发",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
return
|
||||
p_cam = np.array(
|
||||
[float(msg.point.x), float(msg.point.y), float(msg.point.z)], dtype=float
|
||||
)
|
||||
T_ee = self._lookup_T(self.base_frame, self.ee_frame)
|
||||
p_base = self._feature_in_base(T_ee, p_cam) if T_ee is not None else None
|
||||
at_lock = True
|
||||
if self.ee_z_m > 0.0 and T_ee is not None:
|
||||
at_lock = abs(float(T_ee[2, 3]) - self.ee_z_m) <= max(
|
||||
0.02, float(self.approach_xy_gate_m)
|
||||
)
|
||||
if not self._he_cmp_done:
|
||||
self._he_cmp_done = True
|
||||
self._log_handeye_tf_vs_yaml()
|
||||
@@ -558,12 +703,9 @@ class ServoNode(TolerantNode):
|
||||
rejected = False
|
||||
with self._lock:
|
||||
was_lost = self._target_lost_flag
|
||||
if self._track_source == "overhead":
|
||||
# 从顶视切回腕部：允许一次重定位，跳变门不拿顶视点比。
|
||||
was_lost = True
|
||||
self._track_source = "wrist"
|
||||
self._p_meas[:] = p_cam
|
||||
if p_base is not None and at_lock and self._xy_armed:
|
||||
if p_base is not None:
|
||||
verdict = self._update_feat_track(p_base, was_lost=was_lost, now_m=now_m)
|
||||
if verdict == ACCEPT:
|
||||
self._p_feat_base = np.asarray(p_base, dtype=float).copy()
|
||||
@@ -747,31 +889,28 @@ class ServoNode(TolerantNode):
|
||||
lost = self._target_lost_flag
|
||||
source = self._track_source
|
||||
if self._hybrid:
|
||||
self._tick_d435_arrive()
|
||||
self._tick_still_lock()
|
||||
if self.enable_control and self._d435_oneshot_holds_the_arm():
|
||||
return
|
||||
|
||||
# ----- 启动阶段：**先**下降到锁定高度，甚至在任何检测到来之前。
|
||||
# 在第一个 PointStamped 到达之前 have 一直是 False，所以这是一个纯启动
|
||||
# 阶段：只动 Z 下降，XY 锁死，姿态朝下。
|
||||
if (
|
||||
self.enable_control
|
||||
and self.ee_z_m > 0.0
|
||||
and not have
|
||||
and not self._xy_armed
|
||||
):
|
||||
# 还没有目标、人又停在锁高下面：只抬一次 Z。已经有 D435 点或腕部点之后
|
||||
# 不再插这一步 —— 平移途中改高度会抖，也会把正在走的 XY 掐掉。
|
||||
if self.enable_control and self.ee_z_m > 0.0 and not have:
|
||||
T_ee0 = self._lookup_T(self.base_frame, self.ee_frame)
|
||||
if T_ee0 is None:
|
||||
return # TF 还没就绪 —— 等待，不要触发保持急停
|
||||
self._update_xy_arm(float(T_ee0[2, 3]))
|
||||
return
|
||||
z_now = float(T_ee0[2, 3])
|
||||
self._update_xy_arm(z_now)
|
||||
if not self._xy_armed:
|
||||
self.get_logger().info(
|
||||
f"启动下降至 lock_z {self.ee_z_m:.3f}m "
|
||||
f"当前={T_ee0[2, 3]:.3f}m（还没有目标，XY 锁死）",
|
||||
f"先回锁高 {self.ee_z_m:.3f}m 当前={z_now:.3f}m（还没有目标，只动 Z）",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
self._target_lost_flag = False # 盲降期间允许执行
|
||||
self._target_lost_flag = False
|
||||
self._execute_twist(np.zeros(3, dtype=float), p_cam=None, hold_xy=True)
|
||||
self._lock_feedback(False)
|
||||
return
|
||||
self._lock_feedback(False)
|
||||
return
|
||||
|
||||
# ----- Seek：已在锁定高度但还没有有效目标。eye-in-hand 相机只能看到
|
||||
# 一小块区域，所以扫描工作区（先漂到标称工作 XY，再向外螺旋）直到工件进入视野。
|
||||
@@ -821,13 +960,10 @@ class ServoNode(TolerantNode):
|
||||
seen_age = now_mono - last_seen
|
||||
stamp_age = now_sec - stamp if stamp >= 0.0 else 1e9
|
||||
if self._hybrid and source == "overhead":
|
||||
if seen_age > self.overhead_hold_s:
|
||||
self._lock_feedback(False)
|
||||
self._watchdog_trip(
|
||||
f"D435 一次性设定已过期 {seen_age:.1f}s > {self.overhead_hold_s:.1f}s"
|
||||
)
|
||||
return
|
||||
stamp_age = 0.0 # 一次性不是流：别按图像戳判过期
|
||||
# 一次性设定一直用到 D405 接上。按秒数冻住的话，臂还在平移就被看门狗
|
||||
# 掐掉，D435 又不再拍，停在半路。
|
||||
stamp_age = 0.0
|
||||
seen_age = 0.0
|
||||
elif seen_age > self.watchdog_timeout or stamp_age > self.lost_timeout:
|
||||
self._watchdog_trip(
|
||||
f"目标丢失 seen_age={seen_age:.3f}s stamp_age={stamp_age:.3f}s"
|
||||
@@ -1176,12 +1312,343 @@ class ServoNode(TolerantNode):
|
||||
self._xy_in_dead = dead
|
||||
return dead
|
||||
|
||||
def _jacobian_at(self, p_ee: np.ndarray):
|
||||
link_R: List[np.ndarray] = []
|
||||
link_p: List[np.ndarray] = []
|
||||
for fr in self.link_frames:
|
||||
Tl = self._lookup_T(self.base_frame, fr)
|
||||
if Tl is None:
|
||||
return None
|
||||
link_R.append(Tl[:3, :3])
|
||||
link_p.append(Tl[:3, 3])
|
||||
return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
|
||||
|
||||
def _d435_oneshot_holds_the_arm(self) -> bool:
|
||||
"""D435 到点平移：action 短跳，等结果再重测。D405 主跟踪仍走原来的伺服。"""
|
||||
if self.d435_move_only and self._hybrid and self._track_source == "overhead" and self._d435_settled:
|
||||
# 位移测完了：守着，不许掉进 D405 的 _execute_twist。hold 只在到位那一拍发一次。
|
||||
return True
|
||||
if not (self._hybrid and self._track_source == "overhead" and not self._d435_settled):
|
||||
return False
|
||||
if not self._d435_plan_lock.acquire(blocking=False):
|
||||
return True
|
||||
try:
|
||||
return self._d435_oneshot_holds_locked()
|
||||
finally:
|
||||
self._d435_plan_lock.release()
|
||||
|
||||
def _d435_pin_and_stop_hops(self, why: str) -> None:
|
||||
if not self._d435_pinned:
|
||||
self._send_hold()
|
||||
self._d435_pinned = True
|
||||
self._d435_give_up = True
|
||||
self.get_logger().error(why)
|
||||
|
||||
def _sanitize_d435_traj(self, traj: JointTrajectory, min_dt: float = 0.02) -> JointTrajectory:
|
||||
"""与二阶段 ``_sanitize_traj`` 同一条：time_from_start 必须严格递增。"""
|
||||
last = -min_dt
|
||||
for p in traj.points:
|
||||
t = float(p.time_from_start.sec) + float(p.time_from_start.nanosec) * 1e-9
|
||||
if t < last + min_dt:
|
||||
t = last + min_dt
|
||||
ns = int(round(t * 1e9))
|
||||
p.time_from_start.sec = ns // 1_000_000_000
|
||||
p.time_from_start.nanosec = ns % 1_000_000_000
|
||||
last = t
|
||||
return traj
|
||||
|
||||
def _d435_send_action(
|
||||
self,
|
||||
sched,
|
||||
tag: str,
|
||||
hop_m: float,
|
||||
leftover_m: float,
|
||||
v_peak: float,
|
||||
T_move: float,
|
||||
*,
|
||||
ee_z: float,
|
||||
tilt_deg: float,
|
||||
max_dq: float,
|
||||
) -> None:
|
||||
"""先占住 in-flight 再 ``send_goal_async``。话题插点会把这条掐掉。"""
|
||||
traj = JointTrajectory()
|
||||
traj.header.stamp.sec = 0
|
||||
traj.header.stamp.nanosec = 0
|
||||
traj.joint_names = list(self.joint_names)
|
||||
for qi, qdot, t_sec in sched:
|
||||
pt = JointTrajectoryPoint()
|
||||
pt.positions = [float(v) for v in np.asarray(qi).tolist()]
|
||||
pt.velocities = [float(v) for v in np.asarray(qdot).tolist()]
|
||||
ns = int(round(max(0.0, float(t_sec)) * 1e9))
|
||||
pt.time_from_start.sec = ns // 1_000_000_000
|
||||
pt.time_from_start.nanosec = ns % 1_000_000_000
|
||||
traj.points.append(pt)
|
||||
traj = self._sanitize_d435_traj(traj)
|
||||
goal = FollowJointTrajectory.Goal()
|
||||
goal.trajectory = traj
|
||||
self._d435_inflight = True
|
||||
self._d435_sent = True
|
||||
self._d435_pinned = False
|
||||
fut = self._jtc.send_goal_async(goal)
|
||||
fut.add_done_callback(self._d435_on_goal)
|
||||
self.get_logger().info(
|
||||
f"D435 action 下发 {len(sched)} 点 tag={tag} 本跳={hop_m*1000:.0f}mm "
|
||||
f"剩={leftover_m*1000:.0f}mm v={v_peak*1000:.0f}mm/s T={T_move:.1f}s "
|
||||
f"ee_z={ee_z:.3f} tilt={tilt_deg:.2f}deg max|Δq|={max_dq:.4f}rad "
|
||||
f"—— 结果回来之前不再发下一条"
|
||||
)
|
||||
|
||||
def _d435_on_goal(self, fut) -> None:
|
||||
try:
|
||||
handle = fut.result()
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f"D435 JTC 目标回调失败：{exc}")
|
||||
self._d435_inflight = False
|
||||
self._d435_give_up = True
|
||||
return
|
||||
if handle is None or not bool(getattr(handle, "accepted", False)):
|
||||
self.get_logger().error("JTC 拒绝了 D435 轨迹")
|
||||
self._d435_inflight = False
|
||||
self._d435_give_up = True
|
||||
return
|
||||
handle.get_result_async().add_done_callback(self._d435_on_result)
|
||||
|
||||
def _d435_on_result(self, fut) -> None:
|
||||
ok = True
|
||||
try:
|
||||
r = fut.result()
|
||||
inner = getattr(r, "result", None)
|
||||
code = getattr(inner, "error_code", 0) if inner is not None else 0
|
||||
if code not in (0, None):
|
||||
ok = False
|
||||
msg = getattr(inner, "error_string", "")
|
||||
self.get_logger().error(f"D435 JTC 执行失败 error_code={code} {msg}")
|
||||
except Exception as exc:
|
||||
ok = False
|
||||
self.get_logger().error(f"D435 JTC 结果失败：{exc}")
|
||||
self._d435_inflight = False
|
||||
self._d435_settle_until = time.monotonic() + float(D435_SETTLE_S)
|
||||
self._d435_remeasure_log = True
|
||||
if not ok:
|
||||
self._d435_give_up = True
|
||||
|
||||
def _d435_oneshot_holds_locked(self) -> bool:
|
||||
now = time.monotonic()
|
||||
if self._d435_inflight:
|
||||
return True
|
||||
if now < self._d435_settle_until:
|
||||
return True
|
||||
T_ee = self._lookup_T(self.base_frame, self.ee_frame)
|
||||
q = self._current_joints()
|
||||
if T_ee is None or q is None:
|
||||
return True
|
||||
# 只更新锁高标志。这一段不许把控制权交给回锁高流式。
|
||||
self._update_xy_arm(float(T_ee[2, 3]))
|
||||
with self._lock:
|
||||
goal = None if self._p_feat_base is None else self._p_feat_base[:2].copy()
|
||||
if goal is None:
|
||||
return True
|
||||
p = np.asarray(T_ee[:3, 3], dtype=float)
|
||||
dist = float(np.hypot(float(goal[0]) - p[0], float(goal[1]) - p[1]))
|
||||
arrive = float(self.lock_still_move_m)
|
||||
z_lock = float(self.ee_z_m)
|
||||
tilt = flange_tilt_rad(T_ee[:3, :3])
|
||||
at = at_lock_height(float(p[2]), z_lock, self.approach_xy_gate_m)
|
||||
if self._d435_remeasure_log:
|
||||
self._d435_remeasure_log = False
|
||||
self.get_logger().info(
|
||||
f"D435 重测 leftover={dist*1000:.0f}mm "
|
||||
f"dxy=({(p[0]-goal[0])*1000:+.0f},{(p[1]-goal[1])*1000:+.0f})mm "
|
||||
f"ee_xy=({p[0]:.3f},{p[1]:.3f}) goal=({goal[0]:.3f},{goal[1]:.3f}) "
|
||||
f"ee_z={p[2]:.3f} tilt={math.degrees(tilt):.2f}deg"
|
||||
)
|
||||
if self._d435_give_up:
|
||||
self._d435_pinned = True
|
||||
return True
|
||||
hop_max, _cruise_unused = oneshot_hop_limits(
|
||||
dist, hybrid_cruise_vxy(self.v_slow_xy)
|
||||
)
|
||||
if at and dist <= arrive:
|
||||
if oneshot_needs_verticalize(
|
||||
tilt, dist, arrive, already_done=self._d435_vert_done
|
||||
):
|
||||
return self._d435_send_verticalize(q, T_ee, leftover=dist, tilt=tilt)
|
||||
# action 已经 rest-to-rest。到位成功不许再往话题插 hold，那会再抖一下。
|
||||
self._d435_pinned = True
|
||||
return True
|
||||
if self._d435_full_done and self._d435_fix_done:
|
||||
self._d435_pin_and_stop_hops(
|
||||
f"D435 整段+补刀后还差 {dist*1000:.0f}mm —— 钉住不再补刀"
|
||||
)
|
||||
return True
|
||||
if at and not oneshot_may_send_another(
|
||||
dist, self._d435_last_dist, arrive_m=arrive, hop_max_m=hop_max
|
||||
):
|
||||
self._d435_pin_and_stop_hops(
|
||||
f"D435 上一跳几乎没走近，剩差还是 {dist*1000:.0f}mm "
|
||||
f"—— 钉住不再补刀，等停稳后交回 D405"
|
||||
)
|
||||
return True
|
||||
if not at:
|
||||
z_err = abs(float(p[2]) - z_lock)
|
||||
if self._d435_last_z_err is not None and z_err > self._d435_last_z_err * 0.8:
|
||||
self._d435_z_stall += 1
|
||||
else:
|
||||
self._d435_z_stall = 0
|
||||
self._d435_last_z_err = z_err
|
||||
if self._d435_z_stall >= 2:
|
||||
self._d435_pin_and_stop_hops(
|
||||
f"D435 收锁高连续无进展 当前={p[2]:.3f}m 期望={z_lock:.3f}m"
|
||||
)
|
||||
return True
|
||||
if not self._jtc.server_is_ready():
|
||||
self.get_logger().warn("D435 等 JTC action 就绪", throttle_duration_sec=1.0)
|
||||
return True
|
||||
J = self._jacobian_at(p)
|
||||
if J is None:
|
||||
self.get_logger().warn("D435 平移缺 link TF，等下一拍", throttle_duration_sec=1.0)
|
||||
return True
|
||||
segs = oneshot_full_segments(
|
||||
p, goal, z_lock, gate_m=self.approach_xy_gate_m
|
||||
)
|
||||
if not segs:
|
||||
return True
|
||||
# XY / 收 Z 都不许夹收垂直：冻结 J 会把腕部转动漏成 Y 过冲。
|
||||
wps, planned, asked, tag = oneshot_concat_hold_ori(
|
||||
q, J, segs, R=T_ee[:3, :3]
|
||||
)
|
||||
if self._d435_full_done:
|
||||
tag = "fix"
|
||||
if len(wps) < 2 or asked < 1e-5 or planned < 0.75 * asked:
|
||||
self._d435_pin_and_stop_hops(
|
||||
f"D435 平移规划停滞 {planned*1000:.0f}/{asked*1000:.0f}mm —— 钉住不瞎发"
|
||||
)
|
||||
return True
|
||||
max_dq = float(np.max(np.abs(wps[-1] - q)))
|
||||
if max_dq > 1.0:
|
||||
self._d435_pin_and_stop_hops("D435 平移关节行程 >1 rad，拒绝执行")
|
||||
return True
|
||||
cruise = oneshot_full_cruise_mps(dist, hybrid_cruise_vxy(self.v_slow_xy))
|
||||
sched, T_move, v_peak = smooth_joint_schedule(wps, asked, cruise)
|
||||
if tag in ("full", "fix", "xy"):
|
||||
self._d435_last_dist = dist
|
||||
if not self._d435_full_done:
|
||||
self._d435_full_done = True
|
||||
else:
|
||||
self._d435_fix_done = True
|
||||
self._d435_send_action(
|
||||
sched,
|
||||
tag,
|
||||
asked,
|
||||
dist,
|
||||
v_peak,
|
||||
T_move,
|
||||
ee_z=float(p[2]),
|
||||
tilt_deg=float(np.degrees(tilt)),
|
||||
max_dq=max_dq,
|
||||
)
|
||||
return True
|
||||
|
||||
def _d435_send_verticalize(self, q, T_ee, *, leftover: float, tilt: float) -> bool:
|
||||
"""XY 已到位后单独收法兰朝下。和平移拆开，避免 Y 过冲。"""
|
||||
if not self._jtc.server_is_ready():
|
||||
self.get_logger().warn("D435 收垂直等 JTC action 就绪", throttle_duration_sec=1.0)
|
||||
return True
|
||||
p = np.asarray(T_ee[:3, 3], dtype=float)
|
||||
J = self._jacobian_at(p)
|
||||
if J is None:
|
||||
self.get_logger().warn("D435 收垂直缺 link TF，等下一拍", throttle_duration_sec=1.0)
|
||||
return True
|
||||
wps, _remain = translate_hold_ori_waypoints(
|
||||
q, J, np.zeros(3, dtype=float), R=T_ee[:3, :3], verticalize=True
|
||||
)
|
||||
if len(wps) < 2:
|
||||
self._d435_vert_done = True
|
||||
self.get_logger().info(
|
||||
f"D435 收垂直无需再动 tilt={math.degrees(tilt):.2f}deg"
|
||||
)
|
||||
return True
|
||||
max_dq = float(np.max(np.abs(wps[-1] - q)))
|
||||
if max_dq < 1e-4:
|
||||
self._d435_vert_done = True
|
||||
return True
|
||||
if max_dq > 1.0:
|
||||
self._d435_pin_and_stop_hops("D435 收垂直关节行程 >1 rad，拒绝执行")
|
||||
return True
|
||||
hop = max(0.008, float(tilt) * 0.05)
|
||||
cruise = D435_NEAR_CRUISE_MPS
|
||||
sched, T_move, v_peak = smooth_joint_schedule(wps, hop, cruise)
|
||||
self._d435_vert_done = True
|
||||
self._d435_send_action(
|
||||
sched,
|
||||
"vert",
|
||||
hop,
|
||||
leftover,
|
||||
v_peak,
|
||||
T_move,
|
||||
ee_z=float(p[2]),
|
||||
tilt_deg=float(np.degrees(tilt)),
|
||||
max_dq=max_dq,
|
||||
)
|
||||
return True
|
||||
|
||||
def _tick_d435_arrive(self) -> None:
|
||||
"""走到 D435 那个点，并且法兰按一阶段的静止判据停稳，才允许下发 D405。"""
|
||||
if not self._hybrid:
|
||||
return
|
||||
if self._d435_inflight or time.monotonic() < self._d435_settle_until:
|
||||
self._arrive.reset()
|
||||
return
|
||||
with self._lock:
|
||||
if self._track_source != "overhead" or self._d435_settled:
|
||||
return
|
||||
goal = None if self._p_feat_base is None else self._p_feat_base[:2].copy()
|
||||
if goal is None:
|
||||
return
|
||||
T = self._lookup_T(self.base_frame, self.ee_frame)
|
||||
if T is None:
|
||||
return
|
||||
xy = np.asarray(T[:2, 3], dtype=float)
|
||||
dist = float(np.hypot(float(xy[0]) - goal[0], float(xy[1]) - goal[1]))
|
||||
arrived = dist <= float(self.lock_still_move_m) or self._d435_give_up
|
||||
settled = self._arrive.update(
|
||||
xy, time.monotonic(), tracking_ok=arrived
|
||||
)
|
||||
if not settled:
|
||||
return
|
||||
with self._lock:
|
||||
self._d435_settled = True
|
||||
self._d435_exec_until = 0.0
|
||||
if self.d435_move_only:
|
||||
# 保持 d435_commanding=True，感知不会重拍；D405 被 move_only 挡住。
|
||||
# locked 只让 pipeline 结束一阶段，后面会 skip_stage2。
|
||||
# action 已 rest-to-rest，到位不再往话题插 hold。
|
||||
now_m = time.monotonic()
|
||||
if not self._locked:
|
||||
self._locked = True
|
||||
self.get_logger().info(
|
||||
f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
|
||||
f"{self.lock_still_sec:.0f}s —— D435 位移测试结束，"
|
||||
"不交 D405、不进二阶段"
|
||||
)
|
||||
self._lock_pub.publish(Bool(data=True))
|
||||
self._lock_pub_t = now_m
|
||||
elif now_m - self._lock_pub_t > 1.0:
|
||||
self._lock_pub_t = now_m
|
||||
self._lock_pub.publish(Bool(data=True))
|
||||
return
|
||||
self._publish_d435_commanding(False)
|
||||
self.get_logger().info(
|
||||
f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
|
||||
f"{self.lock_still_sec:.0f}s —— 改回下发 D405"
|
||||
)
|
||||
|
||||
def _tick_still_lock(self) -> None:
|
||||
"""D405 跟住且法兰 XY 连续 still_sec 秒没大幅动 → 锁定进二阶段。
|
||||
|
||||
顶视一次性迁臂不算。门槛与位移上限只从 site 来，这里不再写死 3 / 15mm。
|
||||
"""
|
||||
if not self._hybrid:
|
||||
if not self._hybrid or self.d435_move_only:
|
||||
return
|
||||
T = self._lookup_T(self.base_frame, self.ee_frame)
|
||||
xy = None if T is None else np.asarray(T[:2, 3], dtype=float)
|
||||
@@ -1284,48 +1751,31 @@ class ServoNode(TolerantNode):
|
||||
return out
|
||||
|
||||
def _update_xy_arm(self, ee_z: float) -> bool:
|
||||
"""只有当法兰到达 lock_z（真实深度、自适应悬停）后才做 XY 悬停。"""
|
||||
"""只有法兰落在锁高带内才做 XY。低于锁高要先抬回去，不算到位。"""
|
||||
if self.ee_z_m <= 0.0:
|
||||
self._xy_armed = True
|
||||
self._publish_z_locked(True)
|
||||
return True
|
||||
gate = max(0.02, float(self.approach_xy_gate_m))
|
||||
z = float(ee_z)
|
||||
# 已经在锁高或更低：立刻开 XY，禁止再抬回 0.35（否则二阶段又要降下来）。
|
||||
if z <= float(self.ee_z_m) + gate:
|
||||
if not self._xy_armed:
|
||||
self._xy_armed = True
|
||||
self._feat_kf.initialized = False
|
||||
self._p_feat_base = None
|
||||
self._v_xy_cmd[:] = 0.0
|
||||
self.pid_x.reset()
|
||||
self.pid_y.reset()
|
||||
self.get_logger().warn(
|
||||
f"lock-z 就绪 ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}，不再回抬） "
|
||||
"—— 在实时 handeye 上开始 XY 悬停"
|
||||
)
|
||||
return True
|
||||
err = abs(z - float(self.ee_z_m))
|
||||
if not self._xy_armed:
|
||||
if err <= gate:
|
||||
self._xy_armed = True
|
||||
self._feat_kf.initialized = False
|
||||
self._p_feat_base = None
|
||||
self._v_xy_cmd[:] = 0.0
|
||||
self.pid_x.reset()
|
||||
self.pid_y.reset()
|
||||
self.get_logger().warn(
|
||||
f"已到达 lock-z ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}）"
|
||||
" —— 在实时 handeye 上开始 XY 悬停"
|
||||
)
|
||||
elif err > gate + 0.05:
|
||||
at = at_lock_height(z, self.ee_z_m, self.approach_xy_gate_m)
|
||||
if at and not self._xy_armed:
|
||||
self._xy_armed = True
|
||||
self._v_xy_cmd[:] = 0.0
|
||||
self.pid_x.reset()
|
||||
self.pid_y.reset()
|
||||
self.get_logger().info(
|
||||
f"锁高到位 ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}）"
|
||||
" —— 开始 XY；腕部无目标时 D435 可以拍一张"
|
||||
)
|
||||
elif (not at) and self._xy_armed:
|
||||
self._xy_armed = False
|
||||
self._p_feat_base = None
|
||||
self._feat_kf.initialized = False
|
||||
self._v_xy_cmd[:] = 0.0
|
||||
self.get_logger().info(
|
||||
f"已离开 lock-z 区间 ee_z={z:.3f}m —— XY 保持，重新下降",
|
||||
f"离开锁高 ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}）"
|
||||
" —— 先回高度，XY 暂停",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
self._publish_z_locked(at)
|
||||
return self._xy_armed
|
||||
|
||||
def _seek_velocity(self, p_ee: np.ndarray) -> Optional[np.ndarray]:
|
||||
@@ -1392,7 +1842,8 @@ class ServoNode(TolerantNode):
|
||||
|
||||
now_m = time.monotonic()
|
||||
dt_cmd = now_m - self._last_traj_mono
|
||||
if self._last_traj_mono > 0.0 and dt_cmd < (1.0 / self.traj_stream_hz):
|
||||
min_dt = 1.0 / self.traj_stream_hz
|
||||
if self._last_traj_mono > 0.0 and dt_cmd < min_dt:
|
||||
return
|
||||
dt_use = float(np.clip(dt_cmd if self._last_traj_mono > 0.0 else 0.04, 0.015, 0.08))
|
||||
|
||||
@@ -1422,6 +1873,7 @@ class ServoNode(TolerantNode):
|
||||
# hover：在任意锁定高度上把法兰 XY 停在三维特征点上。
|
||||
# lookat：把光轴命中点（图像中心）驱到特征点上。
|
||||
xy_ok = self._update_xy_arm(float(p_ee[2]))
|
||||
cruise = hybrid_cruise_vxy(self.v_slow_xy)
|
||||
if (
|
||||
xy_ok
|
||||
and (not hold_xy)
|
||||
@@ -1437,19 +1889,10 @@ class ServoNode(TolerantNode):
|
||||
if nerr > clip_xy:
|
||||
err = err * (clip_xy / nerr)
|
||||
nerr = clip_xy
|
||||
# ---- 远处限速：抓错目标时只走一小段 ----
|
||||
# 2026-10-08 真机：一阶段被一串高置信度假 k0 顶掉，伺服 0.7s 内以
|
||||
# v_max_xy(0.45m/s) 把臂从 home (0.686,-0.113) 带到工作盒角落
|
||||
# (0.083,-0.515)，之后 148s 全是"检测漏检"。留一道**与"目标真不真"
|
||||
# 无关**的防线：目标离得远就慢走 —— 真目标（真 k0 离 145mm）只是晚
|
||||
# ~1.8s 到位，假目标则最多把臂带走几厘米。
|
||||
v_xy, why_slow = self._slow_xy_limit(nerr, v_xy)
|
||||
if why_slow:
|
||||
self.get_logger().info(
|
||||
f"{why_slow} —— 限速 {self.v_slow_xy*1000:.0f}mm/s 逼近"
|
||||
"（防追错目标大范围移动）",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
# 锁定前整段同一巡航速度。不再用 50mm 远近两档：
|
||||
# 远处本来就慢，近处一放开就按 kp×误差冲，越近越抖。
|
||||
if self._hybrid and not self._locked:
|
||||
v_xy = min(float(v_xy), cruise)
|
||||
if self._xy_deadzone(nerr, update=False):
|
||||
v_base[0] = 0.0
|
||||
v_base[1] = 0.0
|
||||
@@ -1480,9 +1923,10 @@ class ServoNode(TolerantNode):
|
||||
)
|
||||
|
||||
nxy = float(np.hypot(v_base[0], v_base[1]))
|
||||
if nxy > v_xy > 0.0:
|
||||
v_base[0] *= v_xy / nxy
|
||||
v_base[1] *= v_xy / nxy
|
||||
cap_xy = cruise if (self._hybrid and not self._locked) else v_xy
|
||||
if nxy > cap_xy > 0.0:
|
||||
v_base[0] *= cap_xy / nxy
|
||||
v_base[1] *= cap_xy / nxy
|
||||
|
||||
# 在 base 系里保持法兰 XYZ（可配置，默认约 250 mm）。当 ee_hold_xy
|
||||
# 为真时，视觉算出的 XY 会被覆盖。
|
||||
@@ -1493,28 +1937,33 @@ class ServoNode(TolerantNode):
|
||||
v_base[0] = 0.0
|
||||
if abs(self.ee_y_m - p_ee[1]) < 0.004:
|
||||
v_base[1] = 0.0
|
||||
if (not xy_ok) or hold_xy:
|
||||
# 有目标：XY 平移，Z 钉在这一段开始的高度。没目标才回锁高。
|
||||
translating = (not hold_xy) and self._have_target
|
||||
if (not translating) and ((not xy_ok) or hold_xy):
|
||||
v_base[0] = 0.0
|
||||
v_base[1] = 0.0
|
||||
self._v_xy_cmd[:] = 0.0
|
||||
if self.ee_z_m > 0.0:
|
||||
self.get_logger().info(
|
||||
f"下降至 lock_z {self.ee_z_m:.3f}m 当前={p_ee[2]:.3f}m XY 保持",
|
||||
f"回锁高 {self.ee_z_m:.3f}m 当前={p_ee[2]:.3f}m XY 保持",
|
||||
throttle_duration_sec=1.0,
|
||||
)
|
||||
shaped = self._shape_xy_vel(v_base[:2], dt_use)
|
||||
v_base[0] = float(shaped[0])
|
||||
v_base[1] = float(shaped[1])
|
||||
if self.ee_z_m > 0.0:
|
||||
ez_b = float(self.ee_z_m - p_ee[2])
|
||||
# 已在锁高以下：不再抬臂。二阶段会降到 hole_hover_z。
|
||||
if ez_b > 0.0:
|
||||
v_base[2] = 0.0
|
||||
else:
|
||||
vz_lim = float(self.v_max_z) if xy_ok else max(float(self.v_max_z), 0.08)
|
||||
v_base[2] = float(np.clip(kp * ez_b, -vz_lim, vz_lim))
|
||||
if abs(ez_b) < 0.004:
|
||||
v_base[2] = 0.0
|
||||
if translating:
|
||||
if self._z_hold is None:
|
||||
self._z_hold = float(p_ee[2])
|
||||
v_base[2] = hold_translation_vz(
|
||||
float(p_ee[2]), self._z_hold, kp=kp, v_lim=float(self.v_max_z)
|
||||
)
|
||||
else:
|
||||
self._z_hold = None
|
||||
if self.ee_z_m > 0.0:
|
||||
vz_lim = max(float(self.v_max_z), 0.08)
|
||||
v_base[2] = lock_z_velocity(
|
||||
float(p_ee[2]), self.ee_z_m, kp=kp, v_lim=vz_lim
|
||||
)
|
||||
|
||||
w_base = (
|
||||
self._w_align_flange_down(R_ee)
|
||||
@@ -1523,9 +1972,9 @@ class ServoNode(TolerantNode):
|
||||
)
|
||||
|
||||
p_next = p_ee + v_base * T
|
||||
if self.ee_z_m > 0.0:
|
||||
# 绝不指令到配置平面以下
|
||||
floor = max(self.min_ee_z, self.ee_z_m - 0.02)
|
||||
if self.ee_z_m > 0.0 and not translating:
|
||||
# 向下只许到锁高死区内。已经低于锁高时 v_z>0，这条不挡上抬。
|
||||
floor = float(self.ee_z_m) - 0.004
|
||||
if p_next[2] < floor and v_base[2] < 0.0:
|
||||
v_base[2] = 0.0
|
||||
p_next = p_ee + v_base * T
|
||||
|
||||
@@ -45,6 +45,12 @@ from arm_application.geom_ik import (
|
||||
geometric_jacobian,
|
||||
quat_xyzw_to_R,
|
||||
)
|
||||
from arm_application.path_profile import (
|
||||
FROZEN_J_HOP_MAX_M,
|
||||
path_interp as _path_interp,
|
||||
trap_params as _trap_params,
|
||||
trap_sv as _trap_sv,
|
||||
)
|
||||
from arm_application.handeye_realsense import resolve_realsense_topics
|
||||
from arm_application.gauge_boot import perception_source_of
|
||||
from arm_application.gauge_fsm import drag_verdict, hover_z_guard, wrench_all_zero
|
||||
@@ -82,54 +88,6 @@ def _pt(q: np.ndarray, qdot: np.ndarray, t_sec: float) -> JointTrajectoryPoint:
|
||||
return pt
|
||||
|
||||
|
||||
def _trap_params(dist: float, v: float, a: float) -> Tuple[float, float, float, float, float]:
|
||||
"""Rest-to-rest 梯形：返回 (T, v_peak, t_acc, t_cruise, a)。巡航速度保持 v。"""
|
||||
dist = max(float(dist), 1e-6)
|
||||
v = max(float(v), 1e-4)
|
||||
a = max(float(a), 1e-3)
|
||||
d_acc = v * v / (2.0 * a)
|
||||
if 2.0 * d_acc >= dist:
|
||||
v_peak = math.sqrt(max(a * dist, 1e-12))
|
||||
t_acc = v_peak / a
|
||||
return 2.0 * t_acc, v_peak, t_acc, 0.0, a
|
||||
t_acc = v / a
|
||||
t_cruise = (dist - 2.0 * d_acc) / v
|
||||
return 2.0 * t_acc + t_cruise, v, t_acc, t_cruise, a
|
||||
|
||||
|
||||
def _trap_sv(
|
||||
t: float, dist: float, v_peak: float, t_acc: float, t_cruise: float, a: float,
|
||||
) -> Tuple[float, float]:
|
||||
"""时刻 t 的路径弧长 s 与切向速度。"""
|
||||
t = max(0.0, float(t))
|
||||
t_dec0 = t_acc + t_cruise
|
||||
t_end = t_dec0 + t_acc
|
||||
if t >= t_end:
|
||||
return dist, 0.0
|
||||
if t < t_acc:
|
||||
return 0.5 * a * t * t, a * t
|
||||
if t < t_dec0:
|
||||
return 0.5 * a * t_acc * t_acc + v_peak * (t - t_acc), v_peak
|
||||
td = t - t_dec0
|
||||
s0 = 0.5 * a * t_acc * t_acc + v_peak * t_cruise
|
||||
return s0 + v_peak * td - 0.5 * a * td * td, max(0.0, v_peak - a * td)
|
||||
|
||||
|
||||
def _path_interp(waypoints: List[np.ndarray], u: float) -> Tuple[np.ndarray, np.ndarray]:
|
||||
"""沿关节路点折线插值。u∈[0,1] → (q, dq/du)。"""
|
||||
nseg = len(waypoints) - 1
|
||||
if nseg <= 0:
|
||||
z = np.zeros_like(waypoints[0])
|
||||
return waypoints[0].copy(), z
|
||||
u = float(min(1.0, max(0.0, u)))
|
||||
x = u * nseg
|
||||
i = min(nseg - 1, int(math.floor(x)))
|
||||
a = x - i
|
||||
q = (1.0 - a) * waypoints[i] + a * waypoints[i + 1]
|
||||
dq_du = (waypoints[i + 1] - waypoints[i]) * nseg
|
||||
return q, dq_du
|
||||
|
||||
|
||||
def _sanitize_traj(traj: JointTrajectory, min_dt: float = 0.02) -> JointTrajectory:
|
||||
"""JTC 要求 time_from_start 严格递增；短 hop 保持点曾经倒退导致拒轨。"""
|
||||
last = -min_dt
|
||||
@@ -246,6 +204,15 @@ class Stage2Locate(TolerantNode):
|
||||
self.coarse_border_px = int(stage2.get("coarse_border_px") or 2)
|
||||
# 最后轻降到孔面上方 final_gap_mm（不接触）
|
||||
self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0
|
||||
# 观测下降：对准后降到这个间隙就停，方便看规尖是不是对着孔。
|
||||
# 比 final_gap（贴面 2mm）高一截，对偏了也不会把规压上孔沿。
|
||||
self.observe_gap_m = float(stage2.get("observe_gap_mm") or 15.0) / 1000.0
|
||||
if self.observe_gap_m < self.final_gap_m:
|
||||
self.get_logger().warn(
|
||||
f"observe_gap_mm={self.observe_gap_m*1000:.1f} 比贴面目标 "
|
||||
f"{self.final_gap_m*1000:.1f}mm 还近 —— 收到贴面高度，避免观测下降钻进孔"
|
||||
)
|
||||
self.observe_gap_m = self.final_gap_m
|
||||
# 悬停高度**必须**让工具尖端留在工件面上方（2026-09-24 的教训，完整说明见
|
||||
# gauge_fsm.hover_z_guard）：悬停高是按相机视距定的，而工具（夹爪+通止规）
|
||||
# 比相机长 —— 实测悬停时尖端已经钻进工件 25mm，于是 S05 的横向对准把工件
|
||||
@@ -511,18 +478,10 @@ class Stage2Locate(TolerantNode):
|
||||
return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
|
||||
|
||||
def _w_down(self, R: np.ndarray) -> Tuple[np.ndarray, float]:
|
||||
"""让法兰 +Z 指向世界 -Z（垂直朝下）的角速度向量 + 当前倾角。"""
|
||||
z_now = R[:, 2]
|
||||
z_des = np.array([0.0, 0.0, -1.0])
|
||||
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
|
||||
ang = float(np.arccos(c))
|
||||
if ang < 0.006: # 0.34° 以下不再修，避免原地抖动
|
||||
return np.zeros(3), ang
|
||||
axis = np.cross(z_now, z_des)
|
||||
n = float(np.linalg.norm(axis))
|
||||
axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0])
|
||||
mag = float(np.clip(2.0 * ang, -0.5, 0.5))
|
||||
return axis * mag, ang
|
||||
"""让法兰 +Z 指向世界 -Z。公式只在 ``path_profile.flange_down_w``。"""
|
||||
from arm_application.path_profile import flange_down_w
|
||||
|
||||
return flange_down_w(R)
|
||||
|
||||
def _wait_state(self, timeout_s: float) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]:
|
||||
t0 = time.monotonic()
|
||||
@@ -595,7 +554,7 @@ class Stage2Locate(TolerantNode):
|
||||
target: np.ndarray,
|
||||
verticalize: bool,
|
||||
tag: str,
|
||||
max_seg: float = 0.08,
|
||||
max_seg: float = FROZEN_J_HOP_MAX_M,
|
||||
v: float = 0.05,
|
||||
min_dur: float = 1.6,
|
||||
) -> bool:
|
||||
@@ -982,9 +941,12 @@ class Stage2Locate(TolerantNode):
|
||||
except Exception:
|
||||
return None
|
||||
# 两孔统一用工件表面深度 z_wp（中值，稳健）。
|
||||
# 孔是垂直孔，XY 位置与深度无关；而 _ring_depth 在通止规逼近目标孔时会采到
|
||||
# 更近的通止规（~88mm vs 面 ~115mm），把孔深拉偏 → 孔基座坐标随手臂移动漂移、
|
||||
# 精对准追不动目标而振荡。倾斜仅 ~0.4°，左右孔 camera-Z 差 <0.5mm，对 XY 影响 <0.2mm。
|
||||
# 不用每孔的环形深度：通止规逼近时环上会采到更近的规（~88mm vs 面 ~115mm），
|
||||
# 孔的基座坐标就跟着臂飘、精对准追不动。
|
||||
# 法兰相对竖直大约 0.4°，但相机光轴相对法兰大约 30°。深度误差会进 XY：
|
||||
# 2026-10-09 观测高度实测 dXY/dz ≈ 0.46 mm/mm。z_wp 必须是工件面，
|
||||
# 不能是规。同一孔在悬停和观测高度上反投影，基座 XY 可以差约 2mm，
|
||||
# 所以下降之后还要在当前高度再对一次（见 _trim_xy_after_descent）。
|
||||
z_left = z_right = z_wp
|
||||
try:
|
||||
left_b = self._cam_to_base(self._px_to_cam(pts[0][0], pts[0][1], z_left, intr))
|
||||
@@ -1360,65 +1322,108 @@ class Stage2Locate(TolerantNode):
|
||||
return np.array([p[0] + off[0], p[1] + off[1]])
|
||||
|
||||
def _tip_offset_from_depth(
|
||||
self, depth_m: np.ndarray, intr: Dict[str, float], Tee: np.ndarray
|
||||
self,
|
||||
depth_m: np.ndarray,
|
||||
intr: Dict[str, float],
|
||||
Tee: np.ndarray,
|
||||
z_wp_cam: Optional[float] = None,
|
||||
) -> Tuple[Optional[np.ndarray], Optional[float], str]:
|
||||
"""从一帧深度里找**规尖**，返回 (规尖相对法兰的基座 XY 偏移, 工具长, 说明)。
|
||||
|
||||
找不到/不可信时返回 (None, None, 原因)。判据（每条都能真的失败）：
|
||||
1. 只看法兰轴线附近的**中央竖带**——规就夹在爪里，不会跑到画面边上；
|
||||
2. 近端连通域要够大（规杆），且连通域内最深 3% 就是**规尖**；
|
||||
3. 反投出的"法兰 z − 规尖 z"必须落在 [tip_len_min, tip_len_max] ——
|
||||
这一条同时挡住"深度读到别的东西"（如台面/工件），是主干守卫。
|
||||
2. 多个连通域里取画面**更靠下**（v 更大）的那一个，不按面积、也不按
|
||||
「最接近配置工具长」——按面积会选爪子；贴配置长会选法兰轴上的假深点
|
||||
（2026-10-09 真机工具长 225mm、偏移约 0，残差 0.14mm 但孔偏了）；
|
||||
3. 有同一帧工件面深度时，只留**比工件面至少近 8mm** 的像素。相机前倾
|
||||
~30°，规尖沿光轴只比面近十来毫米（2026-10-09 观测高度：面 127mm、
|
||||
尖 115mm）。再往前砍 15mm 会把尖切掉，只剩更近的夹爪/杆身（~80mm），
|
||||
反投影偏移 ~2mm，残差 0.2mm 看着达标，针却偏在孔沿上。
|
||||
没给工件面深度时，退回固定近端（默认 150mm）；
|
||||
4. 反投出的"法兰 z − 规尖 z"必须落在 [tip_len_min, tip_len_max]，
|
||||
并且基座 z 要比工件面高出一截 —— 挡住"深度读到台面/工件"。
|
||||
|
||||
⚠️ 工具长这一项**只在单帧内**可信；实测它会跳（192~223mm）——规尖细、反光，
|
||||
掠射角下深度容易丢，丢了以后"最深有效像素"就往上跑、工具长只会**变短**。
|
||||
所以：
|
||||
· **XY 偏移**对这件事不敏感（规杆细且接近竖直，杆上任一像素反投回基座 XY
|
||||
都在杆轴 ~3mm 内）⇒ 逐帧拿来用是稳的；
|
||||
· **工具长**必须挑"端头看得全"的拍（最长那一簇），见 _refresh_tip_offset。
|
||||
XY 偏移对这件事不敏感（杆细且接近竖直），见 _refresh_tip_offset。
|
||||
"""
|
||||
import cv2
|
||||
|
||||
h, w = depth_m.shape
|
||||
# 8mm：3mm 挡不住深度噪声（工件漏进来，工具长测成 296–315mm）；
|
||||
# 15mm 又把真规尖切掉（面 127mm、尖 115mm，只剩夹爪）。两条都在真机上红过。
|
||||
wp_gap = 0.008
|
||||
# z_wp 只在像相机深度时才当上限（真机孔 ~0.13–0.18m）。基座 z~0.03 会把近端切空。
|
||||
_z_wp = float(z_wp_cam) if z_wp_cam is not None else 0.0
|
||||
if 0.08 <= _z_wp <= 0.45:
|
||||
ceiling = min(_z_wp - wp_gap, self.tip_max_m)
|
||||
ceil_s = f"比工件面{_z_wp*1000:.0f}近{wp_gap*1000:.0f}"
|
||||
else:
|
||||
ceiling = self.tip_near_m
|
||||
ceil_s = f"固定近端{self.tip_near_m*1000:.0f}"
|
||||
valid = (depth_m > self.tip_min_m) & (depth_m < self.tip_max_m)
|
||||
near = (valid & (depth_m < self.tip_near_m)).astype(np.uint8)
|
||||
near = (valid & (depth_m < ceiling)).astype(np.uint8)
|
||||
band = np.zeros_like(near)
|
||||
half = max(1, int(self.tip_band_frac * w))
|
||||
c0, c1 = int(intr["cx"]) - half, int(intr["cx"]) + half
|
||||
band[:, max(0, c0):min(w, c1)] = 1
|
||||
near = near & band
|
||||
if int(near.sum()) < self.tip_min_px:
|
||||
return None, None, f"近端(<{self.tip_near_m*1000:.0f}mm)中央竖带内像素 {int(near.sum())} < {self.tip_min_px}"
|
||||
return None, None, (
|
||||
f"近端(<{ceiling*1000:.0f}mm,{ceil_s})中央竖带内像素 "
|
||||
f"{int(near.sum())} < {self.tip_min_px}"
|
||||
)
|
||||
n, lab, stats, _ = cv2.connectedComponentsWithStats(near, 8)
|
||||
if n <= 1:
|
||||
return None, None, "近端无连通域（规不见了吗）"
|
||||
big = max(range(1, n), key=lambda i: stats[i, 4])
|
||||
ys, xs = np.where(lab == big)
|
||||
dv = depth_m[ys, xs]
|
||||
k = np.percentile(dv, 97) # 连通域内最深 3% ≈ 规尖
|
||||
sel = dv >= k - 1e-6
|
||||
u = float(np.median(xs[sel]))
|
||||
v = float(np.median(ys[sel]))
|
||||
y0, y1 = max(0, int(v) - 2), min(h, int(v) + 3)
|
||||
x0, x1 = max(0, int(u) - 2), min(w, int(u) + 3)
|
||||
patch = depth_m[y0:y1, x0:x1]
|
||||
pv = patch[(patch > self.tip_min_m) & (patch < self.tip_max_m)]
|
||||
if pv.size == 0:
|
||||
return None, None, "规尖邻域深度无效"
|
||||
z_tip = float(np.median(pv))
|
||||
try:
|
||||
p_tip = self._cam_to_base(self._px_to_cam(u, v, z_tip, intr))
|
||||
except Exception as exc: # noqa: BLE001
|
||||
return None, None, f"规尖反投影失败（手眼 TF 缺？）: {exc}"
|
||||
tool_len = float(Tee[2, 3]) - float(p_tip[2])
|
||||
if not (self.tip_len_min <= tool_len <= self.tip_len_max):
|
||||
|
||||
# 掩膜已经丢掉贴着工件面的像素。基座 z 再挡一层：反投影落在面上的
|
||||
# 仍不是规尖。裕量 8mm 而不是 20mm——观测高度规尖离面只有约 15mm，
|
||||
# 20mm 会把真尖一起丢掉（2026-10-09）。悬停时尖离面约 75mm，8mm 仍够。
|
||||
surf = float(getattr(self, "surface_z_m", 0.0) or 0.0)
|
||||
above_surface_m = 0.008
|
||||
cands = []
|
||||
stuck = 0
|
||||
for i in range(1, n):
|
||||
if int(stats[i, 4]) < self.tip_min_px:
|
||||
continue
|
||||
ys, xs = np.where(lab == i)
|
||||
dv = depth_m[ys, xs]
|
||||
k = float(np.percentile(dv, 97))
|
||||
sel = dv >= k - 1e-6
|
||||
u = float(np.median(xs[sel]))
|
||||
v = float(np.median(ys[sel]))
|
||||
# 用连通域自己的近端深度，不用周围 5×5：规尖只有几像素宽，
|
||||
# 邻域中值会落到工件面上，尖又被当成面丢掉。
|
||||
z_tip = float(np.median(dv[sel]))
|
||||
try:
|
||||
p_tip = self._cam_to_base(self._px_to_cam(u, v, z_tip, intr))
|
||||
except Exception as exc: # noqa: BLE001
|
||||
return None, None, f"规尖反投影失败（手眼 TF 缺？）: {exc}"
|
||||
tool_len = float(Tee[2, 3]) - float(p_tip[2])
|
||||
if not (self.tip_len_min <= tool_len <= self.tip_len_max):
|
||||
continue
|
||||
if surf > 0.0 and float(p_tip[2]) < surf + above_surface_m:
|
||||
stuck += 1
|
||||
continue
|
||||
off = np.array(
|
||||
[float(p_tip[0]) - float(Tee[0, 3]), float(p_tip[1]) - float(Tee[1, 3])]
|
||||
)
|
||||
cands.append((off, tool_len, u, v))
|
||||
if not cands:
|
||||
if stuck:
|
||||
return None, None, (
|
||||
f"近端点贴着工件面（高出不足 {above_surface_m*1000:.0f}mm），不是规尖"
|
||||
)
|
||||
return None, None, (
|
||||
f"反投影出的工具长 {tool_len*1000:.0f}mm 不在 "
|
||||
f"反投影出的工具长不在 "
|
||||
f"[{self.tip_len_min*1000:.0f},{self.tip_len_max*1000:.0f}]mm —— "
|
||||
f"近端那个物体不是规"
|
||||
)
|
||||
off = np.array([float(p_tip[0]) - float(Tee[0, 3]), float(p_tip[1]) - float(Tee[1, 3])])
|
||||
return off, tool_len, f"px=({u:.0f},{v:.0f}) 工具长 {tool_len*1000:.0f}mm"
|
||||
# v 优先：画面更靠下的才是伸向工件的那一端。工具长只作同分时的次序，
|
||||
# 不拿去贴近 site.yaml —— 贴配置长会把法兰轴上的点当成规尖。
|
||||
off, tool_len, u, v = max(cands, key=lambda c: (c[3], c[1]))
|
||||
return off, tool_len, f"px=({u:.0f},{v:.0f}) 工具长 {tool_len*1000:.0f}mm {ceil_s}"
|
||||
|
||||
def _refresh_tip_offset(self) -> None:
|
||||
"""攒够**一致**样本后，定下并**冻结**「规尖相对法兰」的偏移。
|
||||
@@ -1440,6 +1445,10 @@ class Stage2Locate(TolerantNode):
|
||||
med = np.median(arr, axis=0)
|
||||
inl = np.linalg.norm(arr - med, axis=1) <= self.tip_off_tol
|
||||
if int(inl.sum()) < self.tip_calib_n:
|
||||
self.get_logger().warn(
|
||||
f" 规尖样本 {len(self._tip_samp)} 拍、一致的只有 {int(inl.sum())}/"
|
||||
f"{self.tip_calib_n}（tol={self.tip_off_tol*1000:.0f}mm）—— 还冻不住，继续采"
|
||||
)
|
||||
return
|
||||
idx = np.where(inl)[0]
|
||||
self._tip_off = arr[idx].mean(0)
|
||||
@@ -1540,8 +1549,11 @@ class Stage2Locate(TolerantNode):
|
||||
2. 腕部 FT 接触检测：``force.z`` 相对悬空基线突变 ⇒ 立即停 + 回退 1mm；
|
||||
3. 任一步 ``_goto`` 没走到位（多半是碰到东西/被挡）⇒ 停止，保持当前高度。
|
||||
"""
|
||||
if not self.final_descent_enable:
|
||||
return True, q, Tee
|
||||
# final_descent_enable：降到贴面 final_gap（约 2mm）。
|
||||
# 关掉时（--no-descent）仍做**观测下降**，停在 observe_gap（约 15mm），
|
||||
# 让人能看出规尖偏没偏，又不把 φ6 规压到 φ7 孔沿上。
|
||||
target_gap = self.final_gap_m if self.final_descent_enable else self.observe_gap_m
|
||||
kind = "贴面轻降" if self.final_descent_enable else "观测下降"
|
||||
self._state("descend_above_hole")
|
||||
tip_len = float(self.tool_tip_below_flange_m)
|
||||
surf = float(self.surface_z_m)
|
||||
@@ -1557,7 +1569,7 @@ class Stage2Locate(TolerantNode):
|
||||
return False, q, Tee
|
||||
|
||||
gap0 = float(Tee[2, 3]) - tip_len - surf # 几何间隙（唯一决定下降量的量）
|
||||
descent = gap0 - self.final_gap_m
|
||||
descent = gap0 - target_gap
|
||||
if descent > self.descent_budget_m + 5e-4:
|
||||
self.get_logger().warn(
|
||||
f"轻降超过配置几何预算：按 FK 算需降 {descent * 1000:.1f}mm > "
|
||||
@@ -1585,9 +1597,9 @@ class Stage2Locate(TolerantNode):
|
||||
)
|
||||
|
||||
self.get_logger().info(
|
||||
f"轻降准备: 几何间隙 {gap0 * 1000:.1f}mm → 目标 {self.final_gap_m * 1000:.1f}mm，"
|
||||
f"{kind}准备: 几何间隙 {gap0 * 1000:.1f}mm → 目标 {target_gap * 1000:.1f}mm，"
|
||||
f"需下降 {descent * 1000:.1f}mm（法兰 {Tee[2, 3]:.4f} − 工具长 {tip_len * 1000:.0f}mm"
|
||||
f" − 工件面 {surf:.4f}；很慢很柔）"
|
||||
f" − 工件面 {surf:.4f}；很慢，XY 保持）"
|
||||
)
|
||||
if descent <= 0.001:
|
||||
self.get_logger().info("规尖已在目标间隙附近，无需下降")
|
||||
@@ -1632,7 +1644,7 @@ class Stage2Locate(TolerantNode):
|
||||
self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf
|
||||
return True, q, Tee
|
||||
# 用 FK 现算剩余下降量（不是累加指令）—— 单步没走到也不会积累误差。
|
||||
remaining = (float(Tee[2, 3]) - tip_len - surf) - self.final_gap_m
|
||||
remaining = (float(Tee[2, 3]) - tip_len - surf) - target_gap
|
||||
if remaining <= 1e-4:
|
||||
break
|
||||
step = min(remaining, 0.004) # 每步 ≤4mm，很慢很柔
|
||||
@@ -1653,12 +1665,128 @@ class Stage2Locate(TolerantNode):
|
||||
self.descent_final_gap_m = gap_f
|
||||
lvl = self.get_logger().info if gap_f >= 0.0 else self.get_logger().warn
|
||||
lvl(
|
||||
f"=== 轻降完成: 规尖悬在孔面上方 ≈{gap_f * 1000:.1f}mm（几何：法兰 "
|
||||
f"=== {kind}完成: 规尖悬在孔面上方 ≈{gap_f * 1000:.1f}mm（几何：法兰 "
|
||||
f"{Tee[2, 3]:.4f} − 工具长 {tip_len * 1000:.0f}mm − 工件面 {surf:.4f}；"
|
||||
f"目标 {self.final_gap_m * 1000:.1f}mm，不接触）==="
|
||||
f"目标 {target_gap * 1000:.1f}mm，不接触）==="
|
||||
)
|
||||
return True, q, Tee
|
||||
|
||||
def _trim_xy_after_descent(self, q, Tee, final: Dict[str, Any]):
|
||||
"""下降之后、保持当前高度，用这一高度的规尖和孔再对一次 XY。
|
||||
|
||||
悬停残差不能拿来当下降后的孔位。相机光轴相对法兰约 30°，孔的基座 XY
|
||||
对深度大约 0.5 mm/mm。2026-10-09：同一孔在悬停（相机深度 180mm，残差
|
||||
0.22mm）和观测高度（127mm）上反投影，基座 XY 差约 2mm；法兰 XY 下降时
|
||||
只动了 0.2mm。画面里针在孔的上方一截，主要是 15mm 间隙的透视，横着的
|
||||
那 2mm 才是要补的。
|
||||
|
||||
只用本高度新量的规尖，不沿用悬停时冻住的偏移。总修正超过 8mm 就不动
|
||||
—— 那一档是夹爪/杆身，不是针。量不到规尖也不动，悬停对准的结果留着。
|
||||
"""
|
||||
if not self.tip_vision:
|
||||
return q, Tee, final
|
||||
final_in = final
|
||||
z_hold = float(Tee[2, 3])
|
||||
saved = self._tip_off
|
||||
budget = 0.008
|
||||
moved = 0.0
|
||||
prev: Optional[np.ndarray] = None
|
||||
for it in range(4):
|
||||
if not rclpy.ok():
|
||||
break
|
||||
try:
|
||||
_bgr, depth_m, intr, res = self._snap_holes(skip_center_gate=True)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f"下降后复拍失败，保持悬停对准的位置：{exc}")
|
||||
self._tip_off = saved
|
||||
return q, Tee, final
|
||||
if not res.get("status"):
|
||||
self.get_logger().warn(f"下降后丢孔（{res.get('msg')}），不再改 XY")
|
||||
self._tip_off = saved
|
||||
return q, Tee, final
|
||||
hb = res.get("hole_base")
|
||||
z_wp = float(res.get("z_wp") or 0.0)
|
||||
if hb is None or len(hb) != 2 or z_wp <= 0.0:
|
||||
self.get_logger().warn("下降后没有孔的基座坐标，不再改 XY")
|
||||
self._tip_off = saved
|
||||
return q, Tee, final
|
||||
left_b = np.asarray(hb[0], dtype=float)
|
||||
right_b = np.asarray(hb[1], dtype=float)
|
||||
k1_b, k2_b = (left_b, right_b) if self.k1_label == "left" else (right_b, left_b)
|
||||
tgt = k1_b if self.target_hole == "k1" else k2_b
|
||||
Tee_now = self._lookup(self.base_frame, self.ee_frame)
|
||||
if Tee_now is not None:
|
||||
Tee = Tee_now
|
||||
off, tlen, why = self._tip_offset_from_depth(
|
||||
depth_m, intr, Tee, z_wp_cam=z_wp
|
||||
)
|
||||
if off is None or tlen is None:
|
||||
self.get_logger().warn(f"下降后没量到规尖（{why}），不再改 XY")
|
||||
self._tip_off = saved
|
||||
return q, Tee, final
|
||||
if prev is not None and float(np.linalg.norm(off - prev)) > 0.003:
|
||||
self.get_logger().warn(
|
||||
f"下降后规尖两拍差 {float(np.linalg.norm(off - prev))*1000:.1f}mm，这一拍不动"
|
||||
)
|
||||
prev = np.array(off, dtype=float)
|
||||
continue
|
||||
prev = np.array(off, dtype=float)
|
||||
if it == 0:
|
||||
continue
|
||||
gxy = np.array([
|
||||
float(Tee[0, 3]) + float(off[0]), float(Tee[1, 3]) + float(off[1])
|
||||
])
|
||||
raw = np.array([float(tgt[0]) - gxy[0], float(tgt[1]) - gxy[1]])
|
||||
err = float(np.linalg.norm(raw))
|
||||
final = dict(final)
|
||||
final["residual_mm"] = round(err * 1000.0, 2)
|
||||
final["tip_offset_m"] = [round(float(off[0]), 5), round(float(off[1]), 5)]
|
||||
final["tip_len_m"] = round(float(tlen), 5)
|
||||
final["gauge_xy_m"] = [round(float(gxy[0]), 5), round(float(gxy[1]), 5)]
|
||||
final["gauge_source"] = "视觉规尖(下降后重测)"
|
||||
self._tip_off = np.array(off, dtype=float)
|
||||
self.get_logger().info(
|
||||
f"[下降后 {it}] 工具=({gxy[0]:.4f},{gxy[1]:.4f}) "
|
||||
f"残差={err*1000:.2f}mm {why}"
|
||||
)
|
||||
if err <= self.align_tol:
|
||||
self.get_logger().info(
|
||||
f"下降后 XY 已在容差内（{err*1000:.2f}mm），高度保持 {z_hold:.4f}"
|
||||
)
|
||||
return q, Tee, final
|
||||
if moved + err > budget:
|
||||
self.get_logger().error(
|
||||
f"下降后要对 {err*1000:.1f}mm（已走 {moved*1000:.1f}mm），"
|
||||
f"超过 {budget*1000:.0f}mm —— 不像针，停在原地"
|
||||
)
|
||||
self._tip_off = saved
|
||||
return q, Tee, final_in
|
||||
|
||||
step = raw
|
||||
if err > 0.004:
|
||||
step = raw * (0.004 / err)
|
||||
move = np.array([
|
||||
float(Tee[0, 3]) + float(step[0]),
|
||||
float(Tee[1, 3]) + float(step[1]),
|
||||
z_hold,
|
||||
])
|
||||
ok, q, Tee = self._goto(
|
||||
q, Tee, move, verticalize=True,
|
||||
tol_m=0.0004, tol_tilt=self.vert_tol_rad * 2.0,
|
||||
tag="trim_xy", v=min(float(self.align_speed), 0.010),
|
||||
max_hops=4, settle_s=0.30, strict_xy=True,
|
||||
)
|
||||
self._pin(q)
|
||||
moved += float(np.linalg.norm(step))
|
||||
if not ok:
|
||||
self.get_logger().warn("下降后 XY 修正没走到，停")
|
||||
return q, Tee, final
|
||||
if abs(float(Tee[2, 3]) - z_hold) > 0.002:
|
||||
self.get_logger().error(
|
||||
f"下降后修正把高度从 {z_hold:.4f} 带到了 {float(Tee[2, 3]):.4f}，停下"
|
||||
)
|
||||
return q, Tee, final
|
||||
return q, Tee, final
|
||||
|
||||
def _snap_holes(self, *, skip_center_gate: bool) -> Tuple[np.ndarray, np.ndarray, Dict[str, float], dict]:
|
||||
"""多帧拍孔：先对 N 帧做时间中值得到干净图，再在其上检测螺纹嵌件对。
|
||||
@@ -2059,7 +2187,9 @@ class Stage2Locate(TolerantNode):
|
||||
# 把法兰对到孔上等于把规偏着对过去。偏移量准一次后冻结（刚体量）。
|
||||
tip_src = "法兰+偏置"
|
||||
if self.tip_vision and self._tip_off is None:
|
||||
off, tlen, why = self._tip_offset_from_depth(depth_m, intr, Tee)
|
||||
off, tlen, why = self._tip_offset_from_depth(
|
||||
depth_m, intr, Tee, z_wp_cam=float(z_wp)
|
||||
)
|
||||
if off is not None and tlen is not None:
|
||||
self._tip_samp.append((off, tlen))
|
||||
self._refresh_tip_offset()
|
||||
@@ -2263,6 +2393,10 @@ class Stage2Locate(TolerantNode):
|
||||
okd, q, Tee = self._descend_above_hole(q, Tee)
|
||||
if not okd:
|
||||
self.get_logger().warn("轻降未完全到位，保持当前高度（安全优先）")
|
||||
else:
|
||||
# 悬停残差在下降 60mm 之后会变大约 2mm（相机 30°，同一孔两个高度
|
||||
# 反投影对不齐）。在观测高度重测规尖，只补 XY。
|
||||
q, Tee, final = self._trim_xy_after_descent(q, Tee, final)
|
||||
# 如实汇报**实际**间隙（几何量）——不是把目标值念一遍。停在离面 50mm 时
|
||||
# 也显示 ~50mm，现场才看得出"轻降没做到"（R1.1：不许把没做到说成做到）。
|
||||
gap_txt = (
|
||||
|
||||
@@ -81,6 +81,7 @@ def _launch_setup(context, *args, **kwargs):
|
||||
# 对到孔心"时先别让规往下走 —— 对不准就下压会把规顶在孔沿上（2026-10-08 真机
|
||||
# 顶到 63N）。默认关；验证对准时用 --no-descent 打开。
|
||||
no_descent = _as_bool(LaunchConfiguration("no_descent").perform(context))
|
||||
d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
|
||||
target_hole = LaunchConfiguration("target_hole").perform(context).strip()
|
||||
depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
|
||||
pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
|
||||
@@ -117,8 +118,9 @@ def _launch_setup(context, *args, **kwargs):
|
||||
"insert_depth_mm": float(depth) if depth else -1.0,
|
||||
"screw_pitch_mm": float(pitch) if pitch else -999.0,
|
||||
"skip_stage1": skip1,
|
||||
"skip_stage2": skip2,
|
||||
"skip_stage3": skip3,
|
||||
"skip_stage2": skip2 or d435_only,
|
||||
"skip_stage3": skip3 or d435_only,
|
||||
"d435_move_only": d435_only,
|
||||
# -1=按 site 的 stage2.final_descent_enable；0=强制不降（调试）
|
||||
"final_descent_enable": 0 if no_descent else -1,
|
||||
}
|
||||
@@ -131,6 +133,7 @@ def _launch_setup(context, *args, **kwargs):
|
||||
LogInfo(
|
||||
msg=f"[pipeline] hole={target_hole or 'site'} depth={depth or 'site'} "
|
||||
f"pitch={pitch or 'site'} skip1={skip1} skip2={skip2} skip3={skip3}"
|
||||
+ (" [d435_only：只测 D435 位移]" if d435_only else "")
|
||||
+ (" [no_descent：只对准，不轻降]" if no_descent else "")
|
||||
),
|
||||
*nodes,
|
||||
@@ -156,6 +159,7 @@ def generate_launch_description() -> LaunchDescription:
|
||||
DeclareLaunchArgument("resnap", default_value="false"),
|
||||
# 只验证精对准、不轻降（见 _launch_setup 里的说明）
|
||||
DeclareLaunchArgument("no_descent", default_value="false"),
|
||||
DeclareLaunchArgument("d435_move_only", default_value="false"),
|
||||
DeclareLaunchArgument("preview", default_value="true"),
|
||||
OpaqueFunction(function=_launch_setup),
|
||||
]
|
||||
|
||||
@@ -60,6 +60,10 @@ def _load_site():
|
||||
return merged, path, perception_overrides(data), servo_overrides(data), robot
|
||||
|
||||
|
||||
def _as_bool(s: str) -> bool:
|
||||
return s.strip().lower() in ("1", "true", "yes")
|
||||
|
||||
|
||||
def _handeye_xyz_xyzw(path: str):
|
||||
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
|
||||
|
||||
@@ -171,6 +175,9 @@ def _launch_setup(context, *args, **kwargs):
|
||||
servo_params["lock_hold_sec"] = float(lock_hold_arg)
|
||||
except ValueError:
|
||||
pass
|
||||
d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
|
||||
perc_params["d435_move_only"] = d435_only
|
||||
servo_params["d435_move_only"] = d435_only
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 感知来源：谁来产生"视觉测量"（site.yaml 的 perception.source）
|
||||
@@ -240,6 +247,10 @@ def _launch_setup(context, *args, **kwargs):
|
||||
),
|
||||
LogInfo(msg=f"[site] 感知来源 perception.source = {perc_src}"),
|
||||
]
|
||||
if d435_only:
|
||||
nodes.append(
|
||||
LogInfo(msg="[d435_only] 只测 D435 单次位移：不交 D405、不进二阶段")
|
||||
)
|
||||
if perc_src == "yolo":
|
||||
nodes.append(LogInfo(msg=f"[site] YOLO weights: {weights}"))
|
||||
nodes.append(
|
||||
@@ -345,6 +356,10 @@ def generate_launch_description() -> LaunchDescription:
|
||||
"lock_hold_sec",
|
||||
default_value="",
|
||||
),
|
||||
DeclareLaunchArgument(
|
||||
"d435_move_only",
|
||||
default_value="false",
|
||||
),
|
||||
OpaqueFunction(function=_launch_setup),
|
||||
]
|
||||
)
|
||||
|
||||
@@ -16,6 +16,7 @@
|
||||
<depend>std_msgs</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>trajectory_msgs</depend>
|
||||
<depend>control_msgs</depend>
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
<exec_depend>python3-yaml</exec_depend>
|
||||
<exec_depend>python3-serial</exec_depend>
|
||||
|
||||
@@ -287,6 +287,9 @@ def test_perception_wires_slew_and_reacquire_guards():
|
||||
src = _perception_src()
|
||||
assert "TrackSlewGuard(" in src, "位移上限没接"
|
||||
assert "FarReacquireConfirm(" in src, "远框重捕获确认没接"
|
||||
assert "self.reacquire_far_px =" in src, (
|
||||
"参数读了没存成属性 —— 远框确认会 AttributeError，看门狗跟着急停"
|
||||
)
|
||||
|
||||
|
||||
def _export_site_env():
|
||||
|
||||
@@ -12,8 +12,8 @@
|
||||
所以这里有两条断言，缺一不可：
|
||||
* **结构**：``_step_unscrew`` 在"成功返回"之前必须调用提出动作 —— 顺序错了
|
||||
（比如把提出挪到 S10 里）就等于没做，因为横向动作已经发生了；
|
||||
* **行为**：提出目标是**跟踪高**（``_track_z()`` = 悬停高 + 50mm），不是写死的
|
||||
毫米数；已经在跟踪高上时不动（否则每次换孔白等 1 秒多）。
|
||||
* **行为**：提出目标是**跟踪高**（``_track_z()``：悬停时尖端离面不够才再抬
|
||||
50mm），不是写死的毫米数；已经在跟踪高上时不动（否则每次换孔白等 1 秒多）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
@@ -79,7 +79,7 @@ def test_unscrew_lifts_out_of_the_hole_before_succeeding():
|
||||
|
||||
|
||||
def test_lift_helper_exists_and_uses_the_track_height_idiom():
|
||||
"""提出高度用 ``_track_z()``（悬停高 + 50mm），不写死毫米数。"""
|
||||
"""提出高度用 ``_track_z()``，不写死毫米数。"""
|
||||
fn = _find_method("_lift_clear_of_hole")
|
||||
assert _call_lines(fn, "_track_z"), (
|
||||
"提出高度必须用 _track_z()：这是流程里『工具已离开工件』的既有定义，"
|
||||
|
||||
@@ -18,8 +18,10 @@ from arm_application.handeye_lib import (
|
||||
CameraIntrinsics,
|
||||
ChessboardStabilizer,
|
||||
Rt_to_T,
|
||||
T_from_flexiv_tcp,
|
||||
board_object_points,
|
||||
calibrate_hand_eye,
|
||||
cam_to_flange_via_tool,
|
||||
format_transform_yaml,
|
||||
invert_Rt,
|
||||
is_pose_filled,
|
||||
@@ -420,3 +422,41 @@ def test_is_pose_filled_rejects_wrong_length():
|
||||
|
||||
def test_is_pose_filled_accepts_real_pose():
|
||||
assert is_pose_filled([-1.107, -0.744, 0.982, 1.856, -0.735, 0.947, -0.118])
|
||||
|
||||
|
||||
def test_flexiv_tcp_identity_is_pure_translation():
|
||||
"""Studio tool10120938：姿态单位四元数，只沿法兰 Z 偏 224.5 mm。"""
|
||||
T = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
|
||||
assert np.allclose(T[:3, :3], np.eye(3), atol=1e-12)
|
||||
assert T[2, 3] == pytest.approx(0.2245)
|
||||
|
||||
|
||||
def test_cam_to_flange_via_tool_must_not_leave_t_unchanged():
|
||||
"""忘了乘时 T_4x4 停在 TCP；乘反了相机会到法兰后方。"""
|
||||
T_cam_tool = np.eye(4)
|
||||
T_cam_tool[:3, 3] = [-0.05961995, -0.01523625, -0.09761517]
|
||||
T_flange_tool = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
|
||||
T_cam_flange = cam_to_flange_via_tool(T_cam_tool, T_flange_tool)
|
||||
delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
|
||||
tool_len = float(np.linalg.norm(T_flange_tool[:3, 3]))
|
||||
assert delta == pytest.approx(tool_len, abs=1e-9)
|
||||
assert tool_len > 0.20
|
||||
# 相机在法兰前方（+Z ≈ 126 mm），与 2026-09-04 旧结果同量级。
|
||||
assert T_cam_flange[2, 3] == pytest.approx(0.12688483, abs=1e-8)
|
||||
assert T_cam_flange[0, 3] == pytest.approx(-0.06011995, abs=1e-8)
|
||||
# 负向：乘反了会到法兰后方约 −296 mm。
|
||||
wrong = T_cam_tool @ np.linalg.inv(T_flange_tool)
|
||||
assert wrong[2, 3] < 0.0
|
||||
# 负向：直接把 cam→工具当成 cam→法兰，平移几乎不动。
|
||||
assert float(np.linalg.norm(T_cam_tool[:3, 3] - T_cam_tool[:3, 3])) < 1e-12
|
||||
# 折回去必须还原：T_ct = inv(T_ft) · T_cf
|
||||
T_back = np.linalg.inv(T_flange_tool) @ T_cam_flange
|
||||
assert np.allclose(T_back, T_cam_tool, atol=1e-12)
|
||||
|
||||
|
||||
def test_cam_at_tcp_is_the_tool_pose_in_flange():
|
||||
"""相机与 TCP 重合时，cam→法兰必须等于法兰→工具，不能是它的逆。"""
|
||||
T_ft = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
|
||||
T_cf = cam_to_flange_via_tool(np.eye(4), T_ft)
|
||||
assert np.allclose(T_cf, T_ft, atol=1e-12)
|
||||
assert T_cf[2, 3] == pytest.approx(0.2245)
|
||||
|
||||
@@ -339,6 +339,7 @@ class TestRunHandeyeScriptMode:
|
||||
assert "camera=d435" in out
|
||||
assert "11x8 inner, square=0.02 m" in out
|
||||
assert "89_d435_eye_to_hand.yaml" in out
|
||||
assert "tool10120938" in out
|
||||
# 负向：不能再去开腕部相机。
|
||||
assert "camera=d405" not in out
|
||||
|
||||
@@ -346,7 +347,8 @@ class TestRunHandeyeScriptMode:
|
||||
out = _show("eye_in_hand", _real_site)
|
||||
assert "mode=eye_in_hand" in out
|
||||
assert "d405" in out
|
||||
assert "8x11 inner, square=0.015 m" in out
|
||||
assert "11x8 inner, square=0.015 m" in out
|
||||
assert "tool10120938" in out
|
||||
assert "89_eye_in_hand.yaml" in out
|
||||
|
||||
def test_missing_board_blocks_calibrate(self, tmp_path: Path):
|
||||
@@ -392,6 +394,58 @@ class TestRunHandeyeScriptMode:
|
||||
assert proc.returncode != 0
|
||||
assert "没有配置 [eye_to_hand] 用的标定板尺寸" in (proc.stdout + proc.stderr)
|
||||
|
||||
def test_left_arm_calibrate_wires_tool10120938(self):
|
||||
"""89 必须按 tool10120938 求解；脚本和节点都要接到这份 TCP。"""
|
||||
cal = (
|
||||
ROOT / "src" / "arm_application" / "arm_application" / "handeye_calibrate.py"
|
||||
).read_text(encoding="utf-8")
|
||||
sh = SCRIPT.read_text(encoding="utf-8")
|
||||
assert "cam_to_flange_via_tool(" in cal
|
||||
assert "gripper_tool" in cal
|
||||
assert "gripper_tool:=" in sh
|
||||
assert "T_4x4 仍是 cam→基座，不折" in sh
|
||||
backup = ROOT / "handeye" / "results" / "backup" / "89_eye_in_hand_flange_tool1_20260904.yaml"
|
||||
assert backup.is_file()
|
||||
e2h_bak = ROOT / "handeye" / "results" / "backup" / "89_d435_eye_to_hand_20261009.yaml"
|
||||
assert e2h_bak.is_file()
|
||||
e2h = (ROOT / "handeye" / "results" / "89_d435_eye_to_hand.yaml").read_text(
|
||||
encoding="utf-8"
|
||||
)
|
||||
assert "gripper_tool: tool10120938" in e2h
|
||||
assert "cam→基座" in e2h
|
||||
import re
|
||||
|
||||
m_e2h = re.search(r"translation_m:\s*\[([^]]+)\]", e2h)
|
||||
assert m_e2h, e2h
|
||||
xyz_e2h = [float(x) for x in m_e2h.group(1).split(",")]
|
||||
# 顶置相机在基座上方约 0.7 m；折错或写到法兰会差几十厘米。
|
||||
assert 0.60 < xyz_e2h[2] < 0.90, xyz_e2h
|
||||
# 眼在手外不得走 cam_to_flange_via_tool（相机和基座都在世界里）。
|
||||
e2h_block = cal.split("if eye_to_hand:")[1].split("elif self.gripper_tool:")[0]
|
||||
assert "cam_to_flange_via_tool" not in e2h_block
|
||||
live = ROOT / "handeye" / "results" / "89_eye_in_hand.yaml"
|
||||
text = live.read_text(encoding="utf-8")
|
||||
assert "BACKUP" in text or "backup/" in text
|
||||
# 乘反了 tz 会是负的（法兰后方）；现行必须在法兰前方。
|
||||
import re
|
||||
|
||||
m = re.search(r"translation_m:\s*\[([^]]+)\]", text)
|
||||
assert m, text
|
||||
xyz = [float(x) for x in m.group(1).split(",")]
|
||||
assert xyz[2] > 0.10, xyz
|
||||
assert "inv(T_flange_to_tool)" not in text
|
||||
|
||||
def test_teach_and_merge_share_the_same_taught_file(self):
|
||||
"""示教写 A、merge 读 B → 采了点也报 got 0。两边必须用 TAUGHT。"""
|
||||
sh = SCRIPT.read_text(encoding="utf-8")
|
||||
teach = (
|
||||
ROOT / "src" / "arm_application" / "arm_application" / "handeye_teach.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert 'poses_file:="${TAUGHT}"' in sh
|
||||
assert "taught_poses_${HANDEYE_MODE}" in sh
|
||||
assert "declare_parameter('poses_file'" in teach
|
||||
assert "get_parameter('poses_file')" in teach
|
||||
|
||||
def test_illegal_mode_exits_immediately(self):
|
||||
proc = subprocess.run(
|
||||
["bash", str(SCRIPT), "--arm", "89", "show", "--mode", "eye_on_foot"],
|
||||
|
||||
@@ -0,0 +1,69 @@
|
||||
"""merge 必须能失败：空文件、连按重复点都不够 5 个独立姿态。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import importlib.util
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[3]
|
||||
_SPEC = importlib.util.spec_from_file_location(
|
||||
"merge_handeye_poses", ROOT / "scripts" / "merge_handeye_poses.py"
|
||||
)
|
||||
assert _SPEC and _SPEC.loader
|
||||
_mod = importlib.util.module_from_spec(_SPEC)
|
||||
sys.modules[_SPEC.name] = _mod
|
||||
_SPEC.loader.exec_module(_mod)
|
||||
|
||||
|
||||
def _pose(i: int) -> list[float]:
|
||||
return [0.1 * i, -1.0, 1.0, 1.4, -0.9, 0.2, 0.4]
|
||||
|
||||
|
||||
def test_parse_taught_txt_skips_comments(tmp_path: Path):
|
||||
text = "\n".join(
|
||||
[
|
||||
"# pose 0 20261012_030533",
|
||||
"-0.730276, -1.078914, 1.182093, 1.378457, -0.914310, 0.247653, 0.409611,",
|
||||
"# image /tmp/a.png",
|
||||
]
|
||||
)
|
||||
p = tmp_path / "taught_parse_once.txt"
|
||||
p.write_text(text, encoding="utf-8")
|
||||
poses = _mod.parse_poses(p)
|
||||
assert len(poses) == 1
|
||||
assert abs(poses[0][0] + 0.730276) < 1e-9
|
||||
|
||||
|
||||
def test_unique_drops_enter_repeat():
|
||||
same = _pose(0)
|
||||
poses = [same, list(same), list(same), _pose(1)]
|
||||
uniq = _mod.unique_poses(poses)
|
||||
assert len(uniq) == 2
|
||||
|
||||
|
||||
def test_merge_refuses_repeat_only_file(tmp_path: Path):
|
||||
src = tmp_path / "taught.txt"
|
||||
dst = tmp_path / "wp.yaml"
|
||||
same = ", ".join(f"{v:.6f}" for v in _pose(0)) + ","
|
||||
src.write_text("\n".join(f"# pose {i}\n{same}" for i in range(12)) + "\n")
|
||||
dst.write_text(" num_poses: 0\n waypoint_positions: []\n", encoding="utf-8")
|
||||
rc = _mod.main(["merge_handeye_poses.py", str(src), str(dst)])
|
||||
assert rc == 4
|
||||
assert "waypoint_positions: []" in dst.read_text(encoding="utf-8")
|
||||
|
||||
|
||||
def test_merge_keeps_five_distinct(tmp_path: Path):
|
||||
src = tmp_path / "taught.txt"
|
||||
dst = tmp_path / "wp.yaml"
|
||||
lines = []
|
||||
for i in range(5):
|
||||
lines.append(f"# pose {i}")
|
||||
lines.append(", ".join(f"{v:.6f}" for v in _pose(i)) + ",")
|
||||
src.write_text("\n".join(lines) + "\n", encoding="utf-8")
|
||||
dst.write_text(" num_poses: 0\n waypoint_positions: []\n", encoding="utf-8")
|
||||
rc = _mod.main(["merge_handeye_poses.py", str(src), str(dst)])
|
||||
assert rc == 0
|
||||
text = dst.read_text(encoding="utf-8")
|
||||
assert "num_poses: 5" in text
|
||||
assert text.count("# pose ") == 5
|
||||
@@ -11,14 +11,49 @@ from types import SimpleNamespace
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.path_profile import (
|
||||
D435_FULL_ACC_M,
|
||||
D435_FULL_CRUISE_MPS,
|
||||
D435_FULL_DEC_M,
|
||||
cosine_peak_acc,
|
||||
smooth_joint_schedule,
|
||||
smooth_path_params,
|
||||
smooth_path_sv,
|
||||
trap_joint_schedule,
|
||||
)
|
||||
from arm_application.overhead_oneshot import (
|
||||
StillLock,
|
||||
at_lock_height,
|
||||
best_det_conf,
|
||||
depth_raw_to_m,
|
||||
k0_uv_from_det,
|
||||
lock_z_velocity,
|
||||
map_uv_to_depth,
|
||||
D405_MISS_BEFORE_D435_S,
|
||||
D435_MOVE_SPEED_SCALE,
|
||||
D435_NEAR_CRUISE_MPS,
|
||||
D435_NEAR_HOP_MAX_M,
|
||||
D435_NEAR_LEFTOVER_M,
|
||||
D435_SETTLE_S,
|
||||
FROZEN_J_HOP_MAX_M,
|
||||
build_arc_xy_move,
|
||||
d405_commands_arm,
|
||||
flange_down_w,
|
||||
hold_translation_vz,
|
||||
hybrid_cruise_vxy,
|
||||
oneshot_cartesian_dp,
|
||||
oneshot_concat_hold_ori,
|
||||
oneshot_full_cruise_mps,
|
||||
oneshot_full_segments,
|
||||
oneshot_hop_limits,
|
||||
oneshot_k0_base,
|
||||
oneshot_may_send_another,
|
||||
oneshot_needs_verticalize,
|
||||
oneshot_shot_timed_out,
|
||||
oneshot_next_hop_m,
|
||||
oneshot_should_submit,
|
||||
select_oneshot_det,
|
||||
trap_acc_for_hop,
|
||||
wrist_needs_handoff,
|
||||
)
|
||||
|
||||
@@ -38,6 +73,399 @@ class TestHandoff:
|
||||
assert wrist_needs_handoff(0.5, 0.5) is False
|
||||
assert wrist_needs_handoff(0.87, 0.5) is False
|
||||
|
||||
def test_below_lock_height_is_not_ready_and_must_lift(self):
|
||||
"""0.26 m 是观测下降停的高度，不是锁高。必须判没到位，并且速度朝上。"""
|
||||
assert at_lock_height(0.257, 0.320, 0.040) is False
|
||||
assert at_lock_height(0.327, 0.320, 0.040) is True
|
||||
assert at_lock_height(0.250, 0.320, 0.040) is False
|
||||
vz = lock_z_velocity(0.257, 0.320, kp=1.0, v_lim=0.08)
|
||||
assert vz > 0.0, "低于锁高必须上抬，写成 0 会和 D435 拒拍卡死"
|
||||
assert lock_z_velocity(0.360, 0.320, kp=1.0, v_lim=0.08) < 0.0
|
||||
assert lock_z_velocity(0.321, 0.320, kp=1.0, v_lim=0.08) == 0.0
|
||||
|
||||
def test_z_locked_qos_is_latched(self):
|
||||
"""晚订阅必须能拿到已经发出的锁高状态，否则 D435 会一直拒拍。"""
|
||||
from rclpy.qos import DurabilityPolicy
|
||||
|
||||
from arm_application.overhead_oneshot import z_locked_qos
|
||||
|
||||
qos = z_locked_qos()
|
||||
assert qos.durability == DurabilityPolicy.TRANSIENT_LOCAL
|
||||
assert qos.depth == 1
|
||||
|
||||
def test_oneshot_waits_half_second_then_fires_once(self):
|
||||
"""连续 0.5 s 没有高置信度才拍。差一帧不算。拍成之后不许连拍。
|
||||
|
||||
不再另等「已到锁高」——那一闸和回锁高互相卡住。
|
||||
"""
|
||||
wait = D405_MISS_BEFORE_D435_S
|
||||
assert wait == 0.5
|
||||
assert oneshot_should_submit(
|
||||
handoff=True, low_for_s=wait - 0.01, already_shot=False
|
||||
) is False
|
||||
assert oneshot_should_submit(
|
||||
handoff=True, low_for_s=wait, already_shot=False
|
||||
) is True
|
||||
assert oneshot_should_submit(
|
||||
handoff=True, low_for_s=wait + 1.0, already_shot=True
|
||||
) is False
|
||||
assert oneshot_should_submit(
|
||||
handoff=False, low_for_s=wait + 1.0, already_shot=False
|
||||
) is False
|
||||
# 拍了伺服没接住（工作盒拒）必须能再拍；已经在赶路不许清。
|
||||
assert oneshot_shot_timed_out(
|
||||
shot=True, commanding=False, age_s=0.79
|
||||
) is False
|
||||
assert oneshot_shot_timed_out(
|
||||
shot=True, commanding=False, age_s=0.80
|
||||
) is True
|
||||
assert oneshot_shot_timed_out(
|
||||
shot=True, commanding=True, age_s=2.0
|
||||
) is False
|
||||
perc = (
|
||||
__import__("pathlib").Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "perception_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert "oneshot_shot_timed_out(" in perc
|
||||
assert "伺服没接住" in perc
|
||||
|
||||
def test_d405_points_stay_off_the_arm_until_arrived_and_settled(self):
|
||||
"""用着 D435 那个点时，到位并且停稳之前，D405 的点不下发。推理不在这里关。"""
|
||||
assert d405_commands_arm(using_d435=False, arrived=False, settled=False) is True
|
||||
assert d405_commands_arm(using_d435=True, arrived=False, settled=False) is False
|
||||
assert d405_commands_arm(using_d435=True, arrived=True, settled=False) is False
|
||||
assert d405_commands_arm(using_d435=True, arrived=False, settled=True) is False
|
||||
assert d405_commands_arm(using_d435=True, arrived=True, settled=True) is True
|
||||
src = (
|
||||
__import__("pathlib").Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "servo_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert "d405_commands_arm(" in src
|
||||
assert "_arrive" in src
|
||||
perc = (
|
||||
__import__("pathlib").Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "perception_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert "D405_MISS_BEFORE_D435_S" in perc
|
||||
assert "d435_commanding" in perc
|
||||
|
||||
def test_d435_move_only_never_hands_back_to_d405(self):
|
||||
"""只测位移：D405 点一律不下发，到位停稳也不交。默认路径仍会交回。"""
|
||||
assert d405_commands_arm(
|
||||
using_d435=False, arrived=True, settled=True, move_only=True
|
||||
) is False
|
||||
assert d405_commands_arm(
|
||||
using_d435=True, arrived=True, settled=True, move_only=True
|
||||
) is False
|
||||
assert d405_commands_arm(
|
||||
using_d435=True, arrived=True, settled=True, move_only=False
|
||||
) is True
|
||||
root = __import__("pathlib").Path(__file__).resolve().parents[1]
|
||||
servo = (root / "arm_application" / "servo_node.py").read_text(encoding="utf-8")
|
||||
perc = (root / "arm_application" / "perception_node.py").read_text(
|
||||
encoding="utf-8"
|
||||
)
|
||||
pipe = (root / "arm_application" / "pipeline_node.py").read_text(
|
||||
encoding="utf-8"
|
||||
)
|
||||
launch = (root / "launch" / "pipeline.launch.py").read_text(encoding="utf-8")
|
||||
vs = (root / "launch" / "visual_servo.launch.py").read_text(encoding="utf-8")
|
||||
ws = root
|
||||
while ws != ws.parent and not (ws / "scripts" / "run_all.sh").is_file():
|
||||
ws = ws.parent
|
||||
run_all = (ws / "scripts" / "run_all.sh").read_text(encoding="utf-8")
|
||||
assert "move_only=self.d435_move_only" in servo
|
||||
assert "不交 D405、不进二阶段" in servo
|
||||
assert "改回下发 D405" in servo
|
||||
assert "self.d435_move_only" in perc
|
||||
assert "or wrist_needs_handoff(" in perc
|
||||
assert "d435_move_only:={str(self.d435_move_only).lower()}" in pipe
|
||||
assert "skip2 = True" in pipe
|
||||
assert '"d435_move_only": d435_only' in launch
|
||||
assert 'LaunchConfiguration("d435_move_only")' in vs
|
||||
assert '"d435_move_only"' in vs
|
||||
assert "--d435-only" in run_all
|
||||
|
||||
def test_whole_path_uses_one_cruise_and_an_arc(self):
|
||||
"""远处仍 80 mm/s。近处必须另降一档。路径是圆弧，不是对着终点做 P。"""
|
||||
assert D435_MOVE_SPEED_SCALE == 1.0
|
||||
assert hybrid_cruise_vxy(0.08) == pytest.approx(0.08)
|
||||
hop_far, v_far = oneshot_hop_limits(0.20, 0.08)
|
||||
hop_mid, v_mid = oneshot_hop_limits(0.12, 0.08)
|
||||
hop_near, v_near = oneshot_hop_limits(0.074, 0.08)
|
||||
assert hop_far == pytest.approx(FROZEN_J_HOP_MAX_M)
|
||||
assert v_far == pytest.approx(0.08)
|
||||
assert hop_mid == pytest.approx(0.040)
|
||||
assert v_mid == pytest.approx(0.08)
|
||||
assert hop_near == pytest.approx(D435_NEAR_HOP_MAX_M)
|
||||
assert v_near == pytest.approx(D435_NEAR_CRUISE_MPS)
|
||||
assert hop_near == pytest.approx(0.015)
|
||||
assert hop_near < 0.030
|
||||
assert v_near < 0.08
|
||||
assert D435_NEAR_LEFTOVER_M == pytest.approx(0.08)
|
||||
assert D435_SETTLE_S == pytest.approx(0.25)
|
||||
p0 = np.array([0.687, -0.113])
|
||||
p1 = np.array([0.583, -0.502])
|
||||
move = build_arc_xy_move(p0, p1, v=0.08, a=0.40, now=0.0)
|
||||
assert move.linear is False
|
||||
assert move.length > float(np.linalg.norm(p1 - p0))
|
||||
assert move.v_cruise == pytest.approx(0.08)
|
||||
xy0, v0, done0 = move.sample(0.0)
|
||||
np.testing.assert_allclose(xy0, p0, atol=1e-6)
|
||||
assert float(np.hypot(v0[0], v0[1])) == pytest.approx(0.0, abs=1e-6)
|
||||
assert done0 is False
|
||||
t_mid = 0.5 * (move.t_acc + (move.t_acc + move.t_cruise))
|
||||
xy_m, v_m, _ = move.sample(t_mid)
|
||||
chord = p1 - p0
|
||||
# 中点不在弦上，否则还是直线冲。
|
||||
rel = xy_m - p0
|
||||
off = abs(float(chord[0] * rel[1] - chord[1] * rel[0])) / float(
|
||||
np.linalg.norm(chord)
|
||||
)
|
||||
assert off > 0.005
|
||||
assert float(np.hypot(v_m[0], v_m[1])) == pytest.approx(0.08, abs=0.008)
|
||||
xy1, v1, done1 = move.sample(move.T + 0.01)
|
||||
np.testing.assert_allclose(xy1, p1, atol=2e-3)
|
||||
assert float(np.hypot(v1[0], v1[1])) == pytest.approx(0.0, abs=1e-4)
|
||||
assert done1 is True
|
||||
src = (
|
||||
__import__("pathlib").Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "servo_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert "hybrid_cruise_vxy(" in src
|
||||
assert "_d435_oneshot_holds_the_arm" in src
|
||||
assert "smooth_joint_schedule(" in src
|
||||
assert "oneshot_full_segments(" in src
|
||||
assert "oneshot_concat_hold_ori(" in src
|
||||
assert "oneshot_full_cruise_mps(" in src
|
||||
assert "oneshot_may_send_another(" in src
|
||||
assert "oneshot_hop_limits(" in src
|
||||
assert "oneshot_needs_verticalize(" in src
|
||||
assert "verticalize=near" not in src
|
||||
oneshot = (
|
||||
__import__("pathlib").Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "overhead_oneshot.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert "verticalize=False" in oneshot
|
||||
assert "oneshot_concat_hold_ori" in oneshot
|
||||
assert '_d435_send_verticalize' in src
|
||||
assert 'tag="vert"' in src or '"vert"' in src
|
||||
assert 'tag = "fix"' in src
|
||||
assert "_d435_full_done" in src
|
||||
assert "_d435_fix_done" in src
|
||||
assert "flange_tilt_rad(" in src
|
||||
assert "D435 重测 leftover=" in src
|
||||
assert "dxy=(" in src
|
||||
assert "_d435_remeasure_log" in src
|
||||
assert "到位成功不许再往话题插 hold" in src
|
||||
assert "FollowJointTrajectory" in src
|
||||
assert "send_goal_async(" in src
|
||||
assert "结果回来之前不再发下一条" in src
|
||||
assert "钉住不再补刀" in src
|
||||
hold = src[src.index("def _d435_oneshot_holds_the_arm") : src.index("def _tick_d435_arrive")]
|
||||
assert 'self._track_source == "overhead"' in hold
|
||||
assert "not self._d435_settled" in hold
|
||||
assert "_publish_joint_schedule(" not in hold
|
||||
assert "oneshot_cartesian_dp(" not in hold
|
||||
assert "trap_joint_schedule(" not in hold
|
||||
assert "_hybrid_goal_xy" not in src
|
||||
assert "def _execute_twist(" in src
|
||||
stage2 = (
|
||||
__import__("pathlib").Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "stage2_locate_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert "from arm_application.path_profile import" in stage2
|
||||
assert "FROZEN_J_HOP_MAX_M" in stage2
|
||||
assert "def _trap_params(" not in stage2
|
||||
assert "def _path_interp(" not in stage2
|
||||
|
||||
def test_frozen_j_hop_is_capped_and_same_leftover_must_not_repeat(self):
|
||||
"""400 mm 一次规划会少走；28 mm 同剩差再发就是真机那趟死循环。"""
|
||||
assert FROZEN_J_HOP_MAX_M == pytest.approx(0.08)
|
||||
assert oneshot_next_hop_m(0.402) == pytest.approx(0.08)
|
||||
assert oneshot_next_hop_m(0.028) == pytest.approx(0.028)
|
||||
arrive = 0.015
|
||||
assert oneshot_may_send_another(0.402, None, arrive_m=arrive) is True
|
||||
assert oneshot_may_send_another(0.322, 0.402, arrive_m=arrive) is True
|
||||
assert oneshot_may_send_another(0.028, 0.028, arrive_m=arrive) is False
|
||||
assert oneshot_may_send_another(0.010, 0.028, arrive_m=arrive) is False
|
||||
|
||||
def test_oneshot_dp_lifts_z_first_and_caps_xy(self):
|
||||
"""没到锁高只收 Z；400 mm 的 XY 必须被砍到 80 mm，并且带着锁高。"""
|
||||
gate = 0.040
|
||||
lift = oneshot_cartesian_dp(
|
||||
[0.65, -0.32, 0.26], [0.58, -0.50], 0.320, gate_m=gate
|
||||
)
|
||||
np.testing.assert_allclose(lift[:2], [0.0, 0.0])
|
||||
assert lift[2] > 0.0
|
||||
assert float(np.linalg.norm(lift)) == pytest.approx(0.060, abs=1e-9)
|
||||
xy = oneshot_cartesian_dp(
|
||||
[0.65, -0.11, 0.320], [0.58, -0.50], 0.320, gate_m=gate
|
||||
)
|
||||
assert float(np.linalg.norm(xy)) == pytest.approx(FROZEN_J_HOP_MAX_M)
|
||||
assert abs(float(xy[2])) < 1e-9
|
||||
acc = trap_acc_for_hop(0.08, 0.08)
|
||||
assert acc == pytest.approx(0.08 * 0.08 / (2.0 * 0.0256), abs=1e-6)
|
||||
|
||||
def test_near_leftover_is_short_slow_xy_only(self):
|
||||
"""真机最后一跳 37–75 mm @ 80 mm/s 会晃。近处必须短、慢、不混 Z。"""
|
||||
gate = 0.040
|
||||
dp = oneshot_cartesian_dp(
|
||||
[0.583, -0.465, 0.327], [0.583, -0.502], 0.320, gate_m=gate
|
||||
)
|
||||
leftover = 0.037
|
||||
hop_max, cruise = oneshot_hop_limits(leftover, 0.08)
|
||||
assert leftover <= D435_NEAR_LEFTOVER_M
|
||||
assert hop_max == pytest.approx(0.015)
|
||||
assert cruise == pytest.approx(0.030)
|
||||
assert float(np.linalg.norm(dp)) == pytest.approx(0.015, abs=1e-9)
|
||||
assert abs(float(dp[2])) < 1e-9
|
||||
assert oneshot_needs_verticalize(0.03, 0.010, 0.015) is True
|
||||
assert oneshot_needs_verticalize(0.03, 0.020, 0.015) is False
|
||||
assert oneshot_needs_verticalize(0.03, 0.010, 0.015, already_done=True) is False
|
||||
assert oneshot_needs_verticalize(0.005, 0.010, 0.015) is False
|
||||
R = np.eye(3)
|
||||
R[2, 2] = -1.0
|
||||
w, ang = flange_down_w(R)
|
||||
np.testing.assert_allclose(w, [0.0, 0.0, 0.0])
|
||||
assert ang < 0.006
|
||||
stage2 = (
|
||||
__import__("pathlib").Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "stage2_locate_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert "return flange_down_w(R)" in stage2
|
||||
|
||||
def test_full_segments_do_not_chop_and_keep_z_then_xy(self):
|
||||
"""整段一次走完：200 mm 不许被砍成 80 mm；没到锁高先整段收 Z。"""
|
||||
gate = 0.040
|
||||
segs = oneshot_full_segments(
|
||||
[0.65, -0.11, 0.26], [0.58, -0.50], 0.320, gate_m=gate
|
||||
)
|
||||
assert segs[0][1] == "lift_z"
|
||||
np.testing.assert_allclose(segs[0][0][:2], [0.0, 0.0])
|
||||
assert segs[0][0][2] == pytest.approx(0.060)
|
||||
assert segs[1][1] == "xy"
|
||||
xy_len = float(np.linalg.norm(segs[1][0][:2]))
|
||||
assert xy_len > 0.20
|
||||
assert xy_len > FROZEN_J_HOP_MAX_M
|
||||
near = oneshot_full_segments(
|
||||
[0.583, -0.465, 0.327], [0.583, -0.502], 0.320, gate_m=gate
|
||||
)
|
||||
assert len(near) == 1
|
||||
assert near[0][1] == "xy"
|
||||
assert float(np.linalg.norm(near[0][0])) == pytest.approx(0.037, abs=1e-9)
|
||||
assert abs(float(near[0][0][2])) < 1e-9
|
||||
assert oneshot_full_cruise_mps(0.20, 0.08) == pytest.approx(D435_FULL_CRUISE_MPS)
|
||||
assert oneshot_full_cruise_mps(0.20, 0.08) < 0.08
|
||||
assert oneshot_full_cruise_mps(0.05, 0.08) == pytest.approx(D435_NEAR_CRUISE_MPS)
|
||||
chopped = oneshot_cartesian_dp(
|
||||
[0.65, -0.11, 0.320], [0.58, -0.50], 0.320, gate_m=gate
|
||||
)
|
||||
assert float(np.linalg.norm(chopped)) == pytest.approx(FROZEN_J_HOP_MAX_M)
|
||||
|
||||
def test_smooth_path_is_rest_to_rest_and_slows_at_the_end(self):
|
||||
"""300 mm @ 60 mm/s：头尾速度 0，起步 ≥3 s，峰值加速度必须低于旧的弹射档。"""
|
||||
assert D435_FULL_ACC_M == pytest.approx(0.100)
|
||||
assert D435_FULL_DEC_M == pytest.approx(0.080)
|
||||
assert D435_FULL_CRUISE_MPS == pytest.approx(0.060)
|
||||
a_now = cosine_peak_acc(D435_FULL_CRUISE_MPS, D435_FULL_ACC_M)
|
||||
a_old = cosine_peak_acc(0.08, 0.040)
|
||||
assert a_old > 0.12
|
||||
assert a_now < 0.035
|
||||
assert a_now < 0.30 * a_old
|
||||
T, v_peak, t_acc, t_cruise, d_acc, d_dec = smooth_path_params(0.30, 0.06)
|
||||
assert v_peak == pytest.approx(0.06)
|
||||
assert d_acc == pytest.approx(0.100)
|
||||
assert d_dec == pytest.approx(0.080)
|
||||
assert t_acc > 3.0
|
||||
assert t_cruise > 0.5
|
||||
s0, v0 = smooth_path_sv(0.0, 0.30, v_peak, t_acc, t_cruise, d_acc, d_dec)
|
||||
assert s0 == pytest.approx(0.0, abs=1e-9)
|
||||
assert v0 == pytest.approx(0.0, abs=1e-9)
|
||||
s_mid, v_mid = smooth_path_sv(
|
||||
t_acc + 0.5 * t_cruise, 0.30, v_peak, t_acc, t_cruise, d_acc, d_dec
|
||||
)
|
||||
assert v_mid == pytest.approx(0.06, abs=1e-6)
|
||||
assert 0.10 < s_mid < 0.24
|
||||
s1, v1 = smooth_path_sv(T, 0.30, v_peak, t_acc, t_cruise, d_acc, d_dec)
|
||||
assert s1 == pytest.approx(0.30, abs=1e-6)
|
||||
assert v1 == pytest.approx(0.0, abs=1e-6)
|
||||
s_dec, v_dec = smooth_path_sv(
|
||||
t_acc + t_cruise + 0.5 * (T - t_acc - t_cruise),
|
||||
0.30,
|
||||
v_peak,
|
||||
t_acc,
|
||||
t_cruise,
|
||||
d_acc,
|
||||
d_dec,
|
||||
)
|
||||
assert v_dec < 0.045
|
||||
assert s_dec > 0.20
|
||||
q0 = np.zeros(7)
|
||||
q1 = np.zeros(7)
|
||||
q1[0] = 0.3
|
||||
sched, T_move, vp = smooth_joint_schedule([q0, q1], 0.30, 0.06)
|
||||
assert T_move == pytest.approx(T, abs=1e-6)
|
||||
assert vp == pytest.approx(0.06)
|
||||
assert float(np.linalg.norm(sched[0][1])) == pytest.approx(0.0, abs=1e-9)
|
||||
assert float(np.linalg.norm(sched[-1][1])) == pytest.approx(0.0, abs=1e-9)
|
||||
times = [t for _, _, t in sched]
|
||||
assert all(times[i] < times[i + 1] for i in range(len(times) - 1))
|
||||
|
||||
def test_concat_hold_ori_refuses_to_mix_verticalize(self):
|
||||
"""串段必须 verticalize=False。混进收垂直会把腕部漏成 Y。"""
|
||||
J = np.zeros((6, 7))
|
||||
J[0, 0] = 1.0
|
||||
J[1, 1] = 1.0
|
||||
J[2, 2] = 1.0
|
||||
segs = [
|
||||
(np.array([0.0, 0.0, 0.02]), "lift_z"),
|
||||
(np.array([0.04, -0.03, 0.0]), "xy"),
|
||||
]
|
||||
wps, planned, asked, tag = oneshot_concat_hold_ori(
|
||||
np.zeros(7), J, segs, R=np.eye(3)
|
||||
)
|
||||
assert tag == "full"
|
||||
assert asked == pytest.approx(0.02 + float(np.hypot(0.04, 0.03)))
|
||||
assert len(wps) >= 3
|
||||
src = (
|
||||
__import__("pathlib").Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "overhead_oneshot.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert "verticalize=False" in src
|
||||
assert "oneshot_concat_hold_ori" in src
|
||||
|
||||
def test_oneshot_schedule_is_rest_to_rest_and_times_increase(self):
|
||||
"""一次下发的轨迹：头尾速度 0，时间递增。中途再插点就会把这条掐掉。"""
|
||||
q0 = np.zeros(7)
|
||||
q1 = np.zeros(7)
|
||||
q1[0] = 0.3
|
||||
sched, T_move, v_peak = trap_joint_schedule(
|
||||
[q0, q1], 0.40, 0.08, 0.40
|
||||
)
|
||||
assert T_move > 4.0
|
||||
assert v_peak == pytest.approx(0.08)
|
||||
assert len(sched) > 8
|
||||
assert float(np.linalg.norm(sched[0][1])) == pytest.approx(0.0, abs=1e-9)
|
||||
assert float(np.linalg.norm(sched[-1][1])) == pytest.approx(0.0, abs=1e-9)
|
||||
times = [t for _, _, t in sched]
|
||||
assert times == sorted(times)
|
||||
assert all(times[i] < times[i + 1] for i in range(len(times) - 1))
|
||||
mid = sched[len(sched) // 3]
|
||||
assert float(np.linalg.norm(mid[1])) > 0.01
|
||||
|
||||
def test_translation_holds_the_height_it_started_at(self):
|
||||
"""平移钉住开始时的高度。沉下去要抬回来，已经在 4mm 内就不要再收。"""
|
||||
assert hold_translation_vz(0.327, 0.327, kp=1.0, v_lim=0.05) == 0.0
|
||||
assert hold_translation_vz(0.325, 0.327, kp=1.0, v_lim=0.05) == 0.0
|
||||
assert hold_translation_vz(0.307, 0.327, kp=1.0, v_lim=0.05) > 0.0
|
||||
assert hold_translation_vz(0.347, 0.327, kp=1.0, v_lim=0.05) < 0.0
|
||||
src = (
|
||||
__import__("pathlib").Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "servo_node.py"
|
||||
).read_text(encoding="utf-8")
|
||||
assert "hold_translation_vz(" in src
|
||||
|
||||
def test_threshold_is_the_caller_value_not_hardcoded(self):
|
||||
# 负向：如果函数内部写死 0.5，改门槛 0.7 时 0.6 会判错
|
||||
assert wrist_needs_handoff(0.6, 0.7) is True
|
||||
|
||||
@@ -13,7 +13,7 @@ from __future__ import annotations
|
||||
|
||||
import pytest
|
||||
|
||||
from arm_application.pipeline_node import stage1_timeout_reason
|
||||
from arm_application.pipeline_node import stage1_timeout_reason, track_lock_z
|
||||
|
||||
|
||||
def test_zero_points_blames_perception_not_calibration():
|
||||
@@ -40,3 +40,21 @@ def test_timeout_seconds_and_log_path_are_in_the_message():
|
||||
msg = stage1_timeout_reason(0, 42.0, "/tmp/x.log")
|
||||
assert "42s" in msg
|
||||
assert "/tmp/x.log" in msg
|
||||
|
||||
|
||||
def test_real_arm_locks_track_height_at_hover():
|
||||
"""真机 89：悬停时规尖已经离面约 75mm，跟踪锁在悬停高，对孔前不再抬。"""
|
||||
z = track_lock_z(0.320, 0.223, 0.022)
|
||||
assert z == pytest.approx(0.320)
|
||||
|
||||
|
||||
def test_sim_still_lifts_when_hover_barely_clears_the_part():
|
||||
"""仿真悬停只留约 2mm，不抬就会在跟踪时刮到工件。"""
|
||||
z = track_lock_z(0.657, 0.243, 0.412)
|
||||
assert z == pytest.approx(0.707)
|
||||
|
||||
|
||||
def test_missing_geometry_does_not_pretend_the_hover_is_clear():
|
||||
"""工具长或工件面缺失时必须抬升。缺数还锁在悬停高，等于没检查。"""
|
||||
assert track_lock_z(0.320, 0.0, 0.022) == pytest.approx(0.370)
|
||||
assert track_lock_z(0.320, 0.223, 0.0) == pytest.approx(0.370)
|
||||
|
||||
@@ -314,6 +314,24 @@ def test_goto_failure_stops_the_descent():
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 不许回退到"把目标值当结果汇报"
|
||||
# --------------------------------------------------------------------------- #
|
||||
def test_no_descent_still_drops_to_the_observe_gap():
|
||||
"""--no-descent 不是停在悬停高：要降到观测间隙（15mm），方便看规尖偏没偏。
|
||||
|
||||
不许降到贴面 2mm —— 对偏了再贴面会把规顶在孔沿上。
|
||||
"""
|
||||
st = _StubStage2()
|
||||
st.final_descent_enable = False
|
||||
st.observe_gap_m = 0.015
|
||||
ok, Tee, st = _run(st)
|
||||
|
||||
assert ok is True
|
||||
gap_mm = (Tee[2, 3] - _TOOL_LEN - _SURFACE_Z) * 1000
|
||||
assert abs(gap_mm - 15.0) < 1.0, f"观测下降应停在离面 15mm，实际 {gap_mm:.1f}mm"
|
||||
total_mm = sum(st.steps_mm)
|
||||
assert total_mm < _BUDGET * 1000 - 5.0, "观测下降走到了贴面预算，等于把 --no-descent 吃掉了"
|
||||
assert any("观测下降完成" in m for m in st.logs), "日志要写明这是观测下降，不是贴面轻降"
|
||||
|
||||
|
||||
def test_wired_final_gap_is_reported_from_geometry_not_target():
|
||||
"""收尾要汇报**几何实算**的间隙，不能把目标值（2.0mm）念一遍。
|
||||
|
||||
|
||||
@@ -27,6 +27,7 @@ from __future__ import annotations
|
||||
from typing import Any, Dict, Optional, Tuple
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.stage2_locate_node import Stage2Locate
|
||||
|
||||
@@ -101,9 +102,9 @@ class _Stub:
|
||||
return _R_CB @ np.asarray(p_cam, float) + self._t_cb
|
||||
|
||||
|
||||
def _call(st: _Stub, depth: np.ndarray):
|
||||
def _call(st: _Stub, depth: np.ndarray, z_wp_cam: Optional[float] = None):
|
||||
return Stage2Locate._tip_offset_from_depth( # type: ignore[arg-type]
|
||||
st, depth, _INTR, _TEE
|
||||
st, depth, _INTR, _TEE, z_wp_cam=z_wp_cam
|
||||
)
|
||||
|
||||
|
||||
@@ -143,6 +144,129 @@ def test_offset_is_stable_frame_to_frame():
|
||||
# --------------------------------------------------------------------------- #
|
||||
# 判据各自都要能真的失败（负向用例）
|
||||
# --------------------------------------------------------------------------- #
|
||||
def _gripper_and_rod_frame() -> np.ndarray:
|
||||
"""爪子块头大且更近 + 规杆更细更深：复现 2026-10-09 真机『按面积选爪子』。"""
|
||||
depth = np.full((480, 800), 0.50, np.float32)
|
||||
depth[20:90, 330:470] = 0.07 # 爪子：宽、浅、画面上方
|
||||
for v in range(90, 280):
|
||||
f = min(1.0, (v - 90) / 200.0)
|
||||
depth[v, 392:408] = 0.08 + f * 0.05 # 规杆：细、伸向工件
|
||||
return depth
|
||||
|
||||
|
||||
def test_picks_the_lower_tip_not_the_config_length_blob():
|
||||
"""画面上方、工具长刚好贴着配置的那一块不是规尖。
|
||||
|
||||
2026-10-09 真机：最接近配置 223mm 的点工具长 225mm、偏移约 0，冻住的是
|
||||
法兰轴线。残差 0.14mm 达标，下降后规尖仍偏出约 10mm。规尖在画面更靠下。
|
||||
"""
|
||||
depth = np.full((480, 800), 0.50, np.float32)
|
||||
depth[40:100, 370:430] = 0.080 # 靠上、居中，工具长贴着配置
|
||||
for v in range(200, 301):
|
||||
f = min(1.0, (v - 200) / 200.0)
|
||||
depth[v, 442:458] = 0.10 + f * 0.02 # 靠下、偏出中心
|
||||
st = _Stub()
|
||||
st.tool_tip_below_flange_m = 0.185
|
||||
off, tlen, why = _call(st, depth)
|
||||
assert off is not None and tlen is not None, f"有规尖却没量到：{why}"
|
||||
v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
|
||||
assert v > 180, f"采到的是贴着配置工具长的上方点，不是规尖：{why}"
|
||||
assert abs(float(off[0])) > 0.005, (
|
||||
f"规尖偏出中心，X 偏移不该≈0（那是法兰轴）：{why} off={off}"
|
||||
)
|
||||
|
||||
|
||||
def test_picks_rod_not_larger_gripper():
|
||||
"""中央竖带里爪子面积更大时，必须仍选**更深的规杆**，不能按连通域面积。"""
|
||||
depth = _gripper_and_rod_frame()
|
||||
off, tlen, why = _call(_Stub(), depth)
|
||||
assert off is not None and tlen is not None, f"有规杆却没量到：{why}"
|
||||
assert "px=" in why
|
||||
# 规尖在杆下端 v≈270，爪子在 v≈20–90；采到爪子 v 会 <100
|
||||
v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
|
||||
assert v > 150, f"采到的是爪子不是规尖：{why}"
|
||||
|
||||
|
||||
def test_workpiece_ceiling_keeps_the_real_tip():
|
||||
"""同一帧工件面深度必须抬高近端上限，否则 150mm 会切掉真规尖。
|
||||
|
||||
2026-10-09 真机：孔 camera-Z ≈168mm，固定 tip_near=150mm 把尖切掉，
|
||||
像素在 v=80（杆上半）和 v=188（尖偶尔漏进来）两档跳，6 拍冻不住。
|
||||
上限要留在工件面前方（8mm）：贴着面的像素是工件，不是规。
|
||||
再往前砍 15mm 会把只比面近十来毫米的真尖切掉。
|
||||
"""
|
||||
depth = np.full((480, 800), 0.50, np.float32)
|
||||
for v in range(80, 200):
|
||||
depth[v, 392:408] = 0.08 + (v - 80) / 120.0 * 0.06 # →0.14，在 150mm 内
|
||||
for v in range(200, 280):
|
||||
depth[v, 392:408] = 0.14 + (v - 200) / 80.0 * 0.024 # →0.164，会被 150mm 切掉
|
||||
depth[300:400, 200:600] = 0.172 # 工件面；近端上限 = 面 − 15mm
|
||||
cut, _t0, why0 = _call(_Stub(), depth) # 无 z_wp → 固定 150mm
|
||||
full, tlen, why = _call(_Stub(), depth, z_wp_cam=0.172)
|
||||
assert full is not None and tlen is not None, f"给了工件面仍没量到尖：{why}"
|
||||
v_full = float(why.split("px=(")[1].split(")")[0].split(",")[1])
|
||||
assert v_full > 200, f"有工件面上限仍没采到杆下端：{why}"
|
||||
if cut is not None:
|
||||
v_cut = float(why0.split("px=(")[1].split(")")[0].split(",")[1])
|
||||
assert v_full > v_cut + 20, (
|
||||
f"工件面上限没有比固定 150mm 采到更靠下的尖：cut={why0} full={why}"
|
||||
)
|
||||
|
||||
|
||||
def test_keeps_the_tip_when_it_is_only_slightly_in_front_of_the_surface():
|
||||
"""规尖只比工件面近十来毫米时，必须采尖，不能退回更近的夹爪。
|
||||
|
||||
2026-10-09 观测高度实测：工件面相机深度 127mm，规尖 115mm，夹爪/杆身 80mm。
|
||||
近端砍到「面 − 15mm」会把尖切掉，冻住的是杆身（偏移约 2mm、工具长贴着
|
||||
配置 223mm）。残差 0.2mm 达标，针却偏在孔沿上。
|
||||
"""
|
||||
depth = np.full((480, 800), 0.50, np.float32)
|
||||
depth[140:190, 360:430] = 0.080 # 夹爪：更近、更靠上
|
||||
depth[210:236, 430:470] = 0.118 # 规尖：只比面近 12mm，画面更靠下
|
||||
depth[250:420, 250:600] = 0.130 # 工件面
|
||||
off, tlen, why = _call(_Stub(), depth, z_wp_cam=0.130)
|
||||
assert off is not None and tlen is not None, f"只比面近 12mm 的尖被切掉了：{why}"
|
||||
v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
|
||||
assert v > 200, f"采到的是夹爪不是规尖：{why}"
|
||||
assert abs(float(off[0])) > 0.005, (
|
||||
f"尖偏出中心，不该量成法兰轴：{why} off={off}"
|
||||
)
|
||||
|
||||
|
||||
def test_rejects_pixels_stuck_to_the_workpiece():
|
||||
"""贴着工件面的近端块必须拒绝 —— 那是面，不是规尖。
|
||||
|
||||
2026-10-09 重跑：上限只让 3mm，噪声把工件漏进来，工具长测成 296–315mm
|
||||
（规实际约 190mm），样本互斥，随后两孔中点一跳就被当成工件被推走。
|
||||
"""
|
||||
depth = np.full((480, 800), 0.50, np.float32)
|
||||
depth[200:420, 360:440] = 0.170 # 大块、深度贴着 0.172 的工件面
|
||||
off, tlen, why = _call(_Stub(), depth, z_wp_cam=0.172)
|
||||
assert off is None and tlen is None, f"贴着工件面的块被当成规尖：{why}"
|
||||
assert "工件" in why
|
||||
|
||||
|
||||
def test_rejects_tip_that_lands_on_the_workpiece_surface():
|
||||
"""反投影落在工件面上的点必须丢掉，哪怕相机深度还在近端上限之内。
|
||||
|
||||
2026-10-09：相机深度留了 15mm，工具长仍测成 298mm（配置 223mm），
|
||||
基座 z 贴在工件面上。偏移冻不住 → exit=8 → 观测下降根本不会执行。
|
||||
"""
|
||||
st = _Stub()
|
||||
st.surface_z_m = 1.0 # 高于任何反投影点
|
||||
off, tlen, why = _call(st, _rod_frame())
|
||||
assert off is None and tlen is None, f"贴在工件面上的点被当成规尖：{why}"
|
||||
assert "工件" in why
|
||||
|
||||
|
||||
def test_keeps_tip_that_clears_the_workpiece_surface():
|
||||
"""规尖明显高于工件面时，表面守卫不许把真规尖一起丢掉。"""
|
||||
st = _Stub()
|
||||
st.surface_z_m = 0.05
|
||||
off, tlen, why = _call(st, _rod_frame())
|
||||
assert off is not None and tlen is not None, f"离面足够却被拒：{why}"
|
||||
|
||||
|
||||
def test_rejects_when_nothing_is_near():
|
||||
"""近端什么都没有（规不在视野）⇒ 必须判失败，不能返回一个假偏移。"""
|
||||
depth = np.full((480, 800), 0.50, np.float32) # 全在远端
|
||||
@@ -301,8 +425,113 @@ def test_default_is_tip_vision_on_in_site():
|
||||
(Path(__file__).resolve().parents[3] / "config" / "site.yaml").read_text(encoding="utf-8")
|
||||
)
|
||||
st2 = ((site.get("hole_detect") or {}).get("stage2") or {})
|
||||
robots = ((site.get("robot") or {}).get("robots") or [])
|
||||
arm89 = next(r for r in robots if str(r.get("sn")) == "Rizon4-063989")
|
||||
servo = arm89.get("servo") or {}
|
||||
assert float(servo.get("ee_z_m")) == pytest.approx(
|
||||
float(servo.get("hole_hover_z_m"))
|
||||
), "左臂跟踪锁高必须等于孔悬停高：对孔完成前不许改高度"
|
||||
assert st2.get("align_tip_vision") is True, "默认必须用视觉规尖"
|
||||
assert float(st2.get("align_tol_m")) <= 0.00025 + 1e-9, (
|
||||
f"align_tol_m={st2.get('align_tol_m')} 太松：会放过系统性的对孔偏差"
|
||||
)
|
||||
assert int(st2.get("align_max_iters")) >= 12, "收紧容差后迭代上限要跟上"
|
||||
|
||||
|
||||
class _TrimStub:
|
||||
"""下降后 XY 修正用到的成员。孔在基座里不动，规尖偏移是常数。"""
|
||||
|
||||
tip_vision = True
|
||||
k1_label = "left"
|
||||
target_hole = "k1"
|
||||
align_tol = 0.00025
|
||||
align_speed = 0.02
|
||||
vert_tol_rad = 0.02
|
||||
base_frame = "world"
|
||||
ee_frame = "flange"
|
||||
_tip_off = np.array([0.01, 0.0])
|
||||
_tip_samp: list = []
|
||||
_tip_len_m = 0.22
|
||||
|
||||
def __init__(self, err_xy=(0.0026, -0.0004), tip=None) -> None:
|
||||
self.hole = np.array([0.50 + err_xy[0], -0.10 + err_xy[1], 0.022])
|
||||
self.tip = None if tip is False else np.array([0.0015, -0.0012])
|
||||
self.moves: list = []
|
||||
self._logger = _StubLogger()
|
||||
|
||||
def get_logger(self):
|
||||
return self._logger
|
||||
|
||||
def _lookup(self, *_a):
|
||||
return None
|
||||
|
||||
def _pin(self, _q) -> None:
|
||||
return None
|
||||
|
||||
def _snap_holes(self, *, skip_center_gate: bool):
|
||||
res = {
|
||||
"status": True,
|
||||
"hole_base": [self.hole.tolist(), (self.hole + np.array([0.0, -0.060, 0.0])).tolist()],
|
||||
"z_wp": 0.127,
|
||||
}
|
||||
return None, None, {}, res
|
||||
|
||||
def _tip_offset_from_depth(self, *_a, **_kw):
|
||||
if self.tip is None:
|
||||
return None, None, "没看到规"
|
||||
return self.tip.copy(), 0.222, "px=(447,226)"
|
||||
|
||||
def _goto(self, _q, Tee, target, **_kw):
|
||||
Tee = np.array(Tee, float)
|
||||
self.moves.append(np.array(target, float))
|
||||
Tee[0, 3] = float(target[0])
|
||||
Tee[1, 3] = float(target[1])
|
||||
Tee[2, 3] = float(target[2])
|
||||
return True, _q, Tee
|
||||
|
||||
|
||||
def _trim(st: _TrimStub, z: float = 0.260):
|
||||
import rclpy
|
||||
|
||||
if not rclpy.ok():
|
||||
rclpy.init()
|
||||
Tee = np.eye(4)
|
||||
Tee[0, 3] = 0.50
|
||||
Tee[1, 3] = -0.10
|
||||
Tee[2, 3] = z
|
||||
final = {"ok": True, "residual_mm": 0.22, "gauge_source": "视觉规尖(已冻结)"}
|
||||
return Stage2Locate._trim_xy_after_descent(st, np.zeros(7), Tee, final) # type: ignore[arg-type]
|
||||
|
||||
|
||||
def test_trim_after_descent_corrects_xy_and_holds_z():
|
||||
"""下降后孔位漂了 2mm 级，要在当前高度补上，不许改高度。
|
||||
|
||||
2026-10-09：悬停残差 0.22mm，降到观测高度后同一孔的基座 XY 差约 2.6mm。
|
||||
"""
|
||||
st = _TrimStub()
|
||||
_q, Tee, final = _trim(st)
|
||||
assert abs(float(Tee[2, 3]) - 0.260) < 1e-9, "下降后修正不许改高度"
|
||||
tip = np.array([Tee[0, 3] + st.tip[0], Tee[1, 3] + st.tip[1]])
|
||||
err = float(np.hypot(tip[0] - st.hole[0], tip[1] - st.hole[1]))
|
||||
assert err <= 0.00025 + 1e-6, f"补完针还偏 {err*1000:.2f}mm"
|
||||
assert final["residual_mm"] <= 0.25
|
||||
assert "下降后" in final["gauge_source"]
|
||||
|
||||
|
||||
def test_trim_refuses_a_centimeter_scale_correction():
|
||||
"""要补的量到了厘米，就是采到夹爪了，必须停在原地。"""
|
||||
st = _TrimStub(err_xy=(0.014, 0.0))
|
||||
_q, Tee, final = _trim(st)
|
||||
assert not st.moves, "厘米级修正被执行了"
|
||||
assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
|
||||
assert final["residual_mm"] == 0.22, "拒绝修正时不能把悬停结果改写成没补上的残差"
|
||||
assert any("不像针" in m for m in st._logger.msgs)
|
||||
|
||||
|
||||
def test_trim_does_not_move_when_the_tip_is_missing():
|
||||
"""下降后没量到规尖，保持悬停对准的位置。"""
|
||||
st = _TrimStub(tip=False)
|
||||
_q, Tee, final = _trim(st)
|
||||
assert not st.moves
|
||||
assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
|
||||
assert final["gauge_source"] == "视觉规尖(已冻结)"
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
|
||||
Reference in New Issue
Block a user