Files

131 lines
4.8 KiB
Python

"""诊断包络体参数:打印包络体位置与点云的关系。"""
import os, json
import numpy as np
import mujoco
from pathlib import Path
ROOT = Path(__file__).resolve().parents[1]
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())
# home 姿态
for side in ("left", "right"):
for jn, q in zip(info["arms"][side]["joints"], info["home"]["arm_qpos"][side]):
d.qpos[m.jnt_qposadr[m.joint(jn).id]] = q
mujoco.mj_forward(m, d)
print("=== 包络体定义检查 ===")
print("躯干中心: (0.0, 0.0, 0.9)")
print("躯干半尺寸: (0.15, 0.20, 0.25)")
print(f"实际 base_base_link: {d.xpos[m.body('base_base_link').id]}")
shoulder_l = np.array([0.0, 0.164, 1.2])
shoulder_r = np.array([0.0, -0.164, 1.2])
print(f"\n肩部定义: 左 {shoulder_l}, 右 {shoulder_r}")
print(f"实际肩部: 左 {d.xpos[m.body('arm_left_L1_Link').id]}, 右 {d.xpos[m.body('arm_right_R1_Link').id]}")
flange_l = d.xpos[m.body('arm_left_L8_Link').id]
flange_r = d.xpos[m.body('arm_right_R8_Link').id]
print(f"\n法兰位置: 左 {flange_l}, 右 {flange_r}")
# 检查胶囊是否覆盖手臂
print(f"\n左臂胶囊: 肩 {shoulder_l} → 法兰 {flange_l}, 半径 0.08")
print(f" 长度: {np.linalg.norm(flange_l - shoulder_l):.3f} m")
print(f"右臂胶囊: 肩 {shoulder_r} → 法兰 {flange_r}, 半径 0.08")
print(f" 长度: {np.linalg.norm(flange_r - shoulder_r):.3f} m")
# 生成点云并检查点分布
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"\n=== 点云与包络体关系 ===")
# 检查躯干区域
torso_center = np.array([0.0, 0.0, 0.9])
torso_half = np.array([0.15, 0.20, 0.25])
in_torso = np.all(np.abs(Pw - torso_center) < torso_half + 0.02, axis=1)
print(f"躯干包络内: {in_torso.sum()} 点")
# 检查左臂胶囊
def capsule_contains(pts, p0, p1, radius, margin=0.02):
v = p1 - p0
L = np.linalg.norm(v)
if L < 1e-6:
return np.linalg.norm(pts - p0, axis=1) < radius + margin
u = v / 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 + margin
in_left_arm = capsule_contains(Pw, shoulder_l, flange_l, 0.08)
in_right_arm = capsule_contains(Pw, shoulder_r, flange_r, 0.08)
print(f"左臂胶囊内: {in_left_arm.sum()} 点")
print(f"右臂胶囊内: {in_right_arm.sum()} 点")
# 检查手区域
hand_l_pos = d.xpos[m.body('left_hand_hand_base_link').id]
hand_r_pos = d.xpos[m.body('right_hand_hand_base_link').id]
hand_l_R = d.xmat[m.body('left_hand_hand_base_link').id].reshape(3, 3)
hand_r_R = d.xmat[m.body('right_hand_hand_base_link').id].reshape(3, 3)
print(f"\n手基座位置: 左 {hand_l_pos}, 右 {hand_r_pos}")
palm_end_l = hand_l_pos + hand_l_R @ np.array([0.15, 0, 0])
palm_end_r = hand_r_pos + hand_r_R @ np.array([0.15, 0, 0])
in_left_hand = capsule_contains(Pw, hand_l_pos, palm_end_l, 0.06)
in_right_hand = capsule_contains(Pw, hand_r_pos, palm_end_r, 0.06)
print(f"左手胶囊内: {in_left_hand.sum()} 点")
print(f"右手胶囊内: {in_right_hand.sum()} 点")
# 指尖
tips_l = [d.xpos[m.body(bn).id] for bn in
["left_hand_thumb_distal", "left_hand_index_distal", "left_hand_middle_distal",
"left_hand_ring_distal", "left_hand_pinky_distal"]]
tips_r = [d.xpos[m.body(bn).id] for bn in
["right_hand_thumb_distal", "right_hand_index_distal", "right_hand_middle_distal",
"right_hand_ring_distal", "right_hand_pinky_distal"]]
in_tips_l = np.zeros(len(Pw), bool)
for tip in tips_l:
in_tips_l |= np.linalg.norm(Pw - tip, axis=1) < 0.025 + 0.02
in_tips_r = np.zeros(len(Pw), bool)
for tip in tips_r:
in_tips_r |= np.linalg.norm(Pw - tip, axis=1) < 0.025 + 0.02
print(f"左手指尖球内: {in_tips_l.sum()} 点")
print(f"右手指尖球内: {in_tips_r.sum()} 点")
# 总计
total_self = in_torso | in_left_arm | in_right_arm | in_left_hand | in_right_hand | in_tips_l | in_tips_r
print(f"\n总自体点: {total_self.sum()} / {len(Pw)} = {total_self.sum()/len(Pw)*100:.1f}%")
# 打印番茄位置作为参照
tomato_pos = d.xpos[m.body('obj_tomato_vis').id] if 'obj_tomato_vis' in [m.body(i).name for i in range(m.nbody)] else None
if tomato_pos is not None:
print(f"\n番茄位置: {tomato_pos}")