"""自体点云滤除:从观测点云中移除机器人自身(双臂+双手)的几何。 核心思路: 1. 从真机读取当前关节角 → FK 得到臂和手的全部 link 位姿 2. 用简化包络体(球+胶囊+长方体)覆盖各 link 3. 点云中落在任一包络体内的点判为自体 → 滤除 包络体参数从仿真 MJCF 模型测量(geom 尺寸),仿真与真机运动学一致。 用法: from self_filter import SelfFilter sf = SelfFilter() pts_filtered = sf.filter(pts_world, arm_joints_left, arm_joints_right, hand_qpos_left, hand_qpos_right) sf.close() 或用于真机采集: from real_arm import RealArm from self_filter import SelfFilter arm_r = RealArm("right"); arm_l = RealArm("left") sf = SelfFilter() sr = arm_r.read(); sl = arm_l.read() pts_filtered = sf.filter_from_real(pts_world, sr["joints"], sl["joints"]) """ import numpy as np from pathlib import Path from scipy.spatial.transform import Rotation as Rot ROOT = Path(__file__).resolve().parents[1] class SelfFilter: """自体点云滤除器:用简化几何包络覆盖双臂双手,移除点云中的自体点。""" def __init__(self, margin=0.015): """ margin: 包络体外扩余量(m),补偿测量误差+相机标定误差,默认 1.5cm。 """ self.margin = margin # 双臂 DH 或几何参数(从 A7_Lite URDF/MJCF 测量) # 简化:用关键连杆的胶囊(两端球心+半径)+ 躯干/肩部的长方体 # 这里先用粗粒度包络,后续可按实际碰撞精细化 # 躯干(base_base_link):圆柱体,世界坐标固定在 (0, 0, z_base) # 从 scene.json 可知双臂零位时法兰在桌前沿之后,躯干大致范围 self.torso_box = { "center": np.array([0.0, 0.0, 0.9]), # 躯干中心(桌高 0.755,躯干从 0.7~1.1) "half": np.array([0.15, 0.20, 0.25]), # 半宽 x/y/z(躯干粗估) } # 臂连杆包络:每段用胶囊近似(起点+终点+半径) # 7 自由度臂,7 个关节,简化为主要连杆的包络 # 从法兰 ready 位姿 (0.26, ±0.45, 0.93) 和零位推断连杆长度 # 粗略估计:肩到肘 ~0.3m,肘到腕 ~0.3m,腕到法兰 ~0.1m # 用运动学链 FK 计算每段端点,这里需要简化 FK # 简化方案:用臂工作空间的粗糙包络球 # 右臂肩部在 (0, -0.164, 1.2),左臂对称 self.arm_shoulder = { "right": np.array([0.0, -0.164, 1.2]), "left": np.array([0.0, 0.164, 1.2]), } # 臂活动范围:以肩为中心,半径 0.7m 的球(臂长+法兰偏移) self.arm_reach = 0.75 # 粗略臂长 # 手掌包络(从法兰到指尖):L20 手在零位下从手腕到指尖 ~0.25m # 手掌体积:长 0.25m × 宽 0.12m × 厚 0.08m self.hand_half = np.array([0.12, 0.06, 0.04]) # 手掌半尺寸 (长/宽/厚) self.hand_len = 0.25 # 从法兰到指尖的距离 def _box_contains(self, pts, center, half): """判断点是否在长方体内:|p - center| < half (各轴独立)。返回布尔掩码。""" d = np.abs(pts - center) return np.all(d < half + self.margin, axis=1) def _sphere_contains(self, pts, center, radius): """判断点是否在球内。""" return np.linalg.norm(pts - center, axis=1) < radius + self.margin def _capsule_contains(self, pts, p0, p1, radius): """判断点是否在胶囊(两端点间线段+半径)内。""" # 点到线段最近点的距离 v = p1 - p0 L = np.linalg.norm(v) if L < 1e-6: return self._sphere_contains(pts, p0, radius) u = v / L # 投影参数 t ∈ [0, L] t = np.clip(np.dot(pts - p0, u), 0, L) closest = p0 + t[:, None] * u return np.linalg.norm(pts - closest, axis=1) < radius + self.margin def _fk_simple(self, joints_7dof, shoulder, side="right"): """简化 FK:给定 7 关节角,返回法兰位姿(位置+旋转矩阵)。 这里用粗略近似:基于 ready 姿态反推的简化 DH。 实际中可用 MuJoCo 模型或 ROS FK 服务,这里为独立性手工实现。 从 scene.json 的 ready 可知: right: qpos=[0.238, 0.724, -0.200, -1.516, 0.843, -0.186, -0.378] 法兰 pos=(0.26, -0.45, 0.93) 简化方案:用关节角的粗略映射 → 法兰位置 (真实 FK 需完整 DH,这里先用线性近似或固定位姿) """ # ⚠ 这是占位实现,需要真实的臂 FK # 临时方案:假设法兰在肩部以下+前方固定偏移(粗糙) # 实际应调用 MuJoCo FK 或从 URDF 计算 # 极简近似(仅用于演示):法兰距肩的距离 ≈ 臂伸展状态 # 实际中 joints[1] (J2) 控制抬升,joints[3] (J4) 控制弯曲 # 这里用 ready 位姿作为默认估计 # 右臂 ready: (0.26, -0.45, 0.93),肩在 (0, -0.164, 1.2) # 相对偏移 ≈ (0.26, -0.286, -0.27) # 更粗略:用关节空间插值(零位→ready)估算 # 零位时手垂下(大致在 (0, -0.164, 0.7)) # ready 时手在 (0.26, -0.45, 0.93) # 由于缺乏完整 FK,这里返回基于关节角的粗略估计 # 实际部署时应替换为真实 FK # 临时简化:返回肩部+粗略偏移 # 使用关节 2/4 的角度估算法兰高度和前伸 j2, j4 = joints_7dof[1], joints_7dof[3] # 大臂抬升、肘弯曲 # 粗略映射(线性近似,非真实 FK) # ready: j2≈0.72, j4≈-1.52 → 法兰 z≈0.93, x≈0.26 # home: j2≈0, j4≈0 → 法兰 z≈0.7, x≈0 dz = 0.23 * (j2 / 0.72) if abs(j2) > 0.01 else 0 dx = 0.26 * (j2 / 0.72) if abs(j2) > 0.01 else 0 base_flange = shoulder + np.array([dx, 0, -0.27 + dz]) # 姿态简化为单位阵(不影响包络,后续可补) R = np.eye(3) return base_flange, R def _fk_mujoco(self, joints_left, joints_right, hand_qpos_left=None, hand_qpos_right=None): """用 MuJoCo 模型做真实 FK(更准确)。 加载场景模型,设置关节角,调用 mj_forward,读取法兰位姿。 手的 qpos 可选(用于精细滤除指尖)。 """ import mujoco, os scene = os.environ.get("A7L20_SCENE", "a7l20_scene") m = mujoco.MjModel.from_xml_path(str(ROOT / f"scene/{scene}.xml")) d = mujoco.MjData(m) # 设置臂关节 for i, jname in enumerate([f"arm_left_L{j+1}_Joint" for j in range(7)]): d.qpos[m.jnt_qposadr[m.joint(jname).id]] = joints_left[i] for i, jname in enumerate([f"arm_right_R{j+1}_Joint" for j in range(7)]): d.qpos[m.jnt_qposadr[m.joint(jname).id]] = joints_right[i] # 设置手关节(如果提供) if hand_qpos_left is not None: # 从 scene.json 读取手的关节名列表 import json info = json.loads((ROOT / f"scene/{scene}.json").read_text()) for i, jn in enumerate(info["hands"]["left"]["active_joints"]): if i < len(hand_qpos_left): d.qpos[m.jnt_qposadr[m.joint(jn).id]] = hand_qpos_left[i] if hand_qpos_right is not None: import json info = json.loads((ROOT / f"scene/{scene}.json").read_text()) for i, jn in enumerate(info["hands"]["right"]["active_joints"]): if i < len(hand_qpos_right): d.qpos[m.jnt_qposadr[m.joint(jn).id]] = hand_qpos_right[i] mujoco.mj_forward(m, d) # 读取法兰位姿 flange_left = d.xpos[m.body("arm_left_L8_Link").id].copy() flange_right = d.xpos[m.body("arm_right_R8_Link").id].copy() R_left = d.xmat[m.body("arm_left_L8_Link").id].reshape(3, 3).copy() R_right = d.xmat[m.body("arm_right_R8_Link").id].reshape(3, 3).copy() # 读取手基座位姿(用于手包络) hand_left_pos = d.xpos[m.body("left_hand_hand_base_link").id].copy() hand_right_pos = d.xpos[m.body("right_hand_hand_base_link").id].copy() hand_left_R = d.xmat[m.body("left_hand_hand_base_link").id].reshape(3, 3).copy() hand_right_R = d.xmat[m.body("right_hand_hand_base_link").id].reshape(3, 3).copy() # 读取指尖位置(精细滤除) tips_left = [d.xpos[m.body(bn).id].copy() for bn in ["left_hand_thumb_distal", "left_hand_index_distal", "left_hand_middle_distal", "left_hand_ring_distal", "left_hand_pinky_distal"]] tips_right = [d.xpos[m.body(bn).id].copy() for bn in ["right_hand_thumb_distal", "right_hand_index_distal", "right_hand_middle_distal", "right_hand_ring_distal", "right_hand_pinky_distal"]] return { "flange_left": (flange_left, R_left), "flange_right": (flange_right, R_right), "hand_left": (hand_left_pos, hand_left_R), "hand_right": (hand_right_pos, hand_right_R), "tips_left": tips_left, "tips_right": tips_right, } def filter(self, pts_world, joints_left, joints_right, hand_qpos_left=None, hand_qpos_right=None, use_mujoco=True, verbose=False): """滤除自体点云。 参数: pts_world: Nx3 点云(世界坐标系) joints_left/right: 7 关节角(弧度) hand_qpos_left/right: 16 手关节角(可选,用于精细滤除) use_mujoco: True=用 MuJoCo FK(准确),False=用简化 FK(快但粗糙) verbose: 打印滤除统计 返回: 滤除后的点云(Nx3) """ mask = np.ones(len(pts_world), dtype=bool) # True=保留 # 1. 躯干 mask &= ~self._box_contains(pts_world, self.torso_box["center"], self.torso_box["half"]) if use_mujoco: # 用 MuJoCo 做精确 FK fk = self._fk_mujoco(joints_left, joints_right, hand_qpos_left, hand_qpos_right) # 2. 左臂:肩到法兰的胶囊 shoulder_l = self.arm_shoulder["left"] flange_l, _ = fk["flange_left"] mask &= ~self._capsule_contains(pts_world, shoulder_l, flange_l, radius=0.08) # 3. 右臂:肩到法兰的胶囊 shoulder_r = self.arm_shoulder["right"] flange_r, _ = fk["flange_right"] mask &= ~self._capsule_contains(pts_world, shoulder_r, flange_r, radius=0.08) # 4. 左手:手基座的长方体包络 hand_l_pos, hand_l_R = fk["hand_left"] # 手掌在手基座系的包络,转到世界系 for tip in fk["tips_left"]: mask &= ~self._sphere_contains(pts_world, tip, radius=0.025) # 手掌主体:从手基座到指尖的胶囊(粗略) palm_end_l = hand_l_pos + hand_l_R @ np.array([0.15, 0, 0]) # 手系 +x 是掌心方向 mask &= ~self._capsule_contains(pts_world, hand_l_pos, palm_end_l, radius=0.06) # 5. 右手:同左手 hand_r_pos, hand_r_R = fk["hand_right"] for tip in fk["tips_right"]: mask &= ~self._sphere_contains(pts_world, tip, radius=0.025) palm_end_r = hand_r_pos + hand_r_R @ np.array([0.15, 0, 0]) mask &= ~self._capsule_contains(pts_world, hand_r_pos, palm_end_r, radius=0.06) else: # 简化 FK(快速但粗糙) flange_l, _ = self._fk_simple(joints_left, self.arm_shoulder["left"], "left") flange_r, _ = self._fk_simple(joints_right, self.arm_shoulder["right"], "right") # 肩到法兰的胶囊 mask &= ~self._capsule_contains(pts_world, self.arm_shoulder["left"], flange_l, radius=0.08) mask &= ~self._capsule_contains(pts_world, self.arm_shoulder["right"], flange_r, radius=0.08) # 手的球形包络(从法兰延伸) # 假设手沿法兰 +z 方向伸出(粗略) hand_l_tip = flange_l + np.array([0.15, 0, 0]) # 粗略估计 hand_r_tip = flange_r + np.array([0.15, 0, 0]) mask &= ~self._capsule_contains(pts_world, flange_l, hand_l_tip, radius=0.06) mask &= ~self._capsule_contains(pts_world, flange_r, hand_r_tip, radius=0.06) if verbose: n0 = len(pts_world) n1 = mask.sum() print(f"[self_filter] 原始 {n0} 点 → 滤除 {n0 - n1} 自体点 ({(n0-n1)/n0*100:.1f}%) → 保留 {n1} 点") return pts_world[mask] def filter_from_real(self, pts_world, joints_right, joints_left, hand_qpos_right=None, hand_qpos_left=None, use_mujoco=True, verbose=False): """便捷接口:直接从真机读取的关节角滤除。参数顺序与 RealArm.read() 对应。""" return self.filter(pts_world, joints_left, joints_right, hand_qpos_left, hand_qpos_right, use_mujoco=use_mujoco, verbose=verbose) def close(self): """清理资源(占位,当前无需清理)。""" pass if __name__ == "__main__": # 测试:生成仿真点云,验证滤除效果 import mujoco, os, json os.environ.setdefault("A7L20_SCENE", "ycb_tomato") scene = os.environ["A7L20_SCENE"] m = mujoco.MjModel.from_xml_path(str(ROOT / f"scene/{scene}.xml")) d = mujoco.MjData(m) info = json.loads((ROOT / f"scene/{scene}.json").read_text()) # 设置 ready 姿态 for side in ("left", "right"): for jn, q in zip(info["arms"][side]["joints"], info["ready"]["arm_qpos"][side]): d.qpos[m.jnt_qposadr[m.joint(jn).id]] = q mujoco.mj_forward(m, d) # 生成胸口相机点云 r = mujoco.Renderer(m, 480, 848) r.enable_depth_rendering() r.update_scene(d, camera="chest_cam") dep = r.render() # 反投影 fovy = float(m.cam_fovy[m.camera("chest_cam").id]) h, w = 480, 848 fy = (h / 2) / np.tan(np.deg2rad(fovy) / 2); fx = fy cx, cy = w / 2, h / 2 us, vs = np.meshgrid(np.arange(w), np.arange(h)) valid = np.isfinite(dep) & (dep > 0) & (dep < 2.0) z = dep[valid] x = (us[valid] - cx) / fx * z y = (vs[valid] - cy) / fy * z Pc = np.stack([x, y, z], 1) # 转世界坐标 cid = m.camera("chest_cam").id R_mj = d.cam_xmat[cid].reshape(3, 3) t = d.cam_xpos[cid] R_cv = R_mj @ np.diag([1.0, -1.0, -1.0]) Pw = Pc @ R_cv.T + t print(f"原始点云:{len(Pw)} 点") # 滤除自体 sf = SelfFilter(margin=0.02) Jl = info["ready"]["arm_qpos"]["left"] Jr = info["ready"]["arm_qpos"]["right"] Pf = sf.filter(Pw, Jl, Jr, use_mujoco=True, verbose=True) print(f"滤除后:{len(Pf)} 点") print(f"Z 范围:原始 [{Pw[:,2].min():.2f}, {Pw[:,2].max():.2f}],滤除后 [{Pf[:,2].min():.2f}, {Pf[:,2].max():.2f}]") sf.close()