Files

338 lines
15 KiB
Python
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
"""自体点云滤除:从观测点云中移除机器人自身(双臂+双手)的几何。
核心思路:
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()