338 lines
15 KiB
Python
338 lines
15 KiB
Python
"""自体点云滤除:从观测点云中移除机器人自身(双臂+双手)的几何。
|
||
|
||
核心思路:
|
||
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()
|