131 lines
4.8 KiB
Python
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}")
|