163 lines
11 KiB
Python
163 lines
11 KiB
Python
"""HUG 人手抓取 (MANO 21 点, 世界系) → L20 左/右手:腕位姿 (hand_base_link) + 16 主动关节角 → grasp_l20_<obj>_<k>.json
|
||
|
||
解释器:$RETARGET_PYTHON(dex-retargeting + pinocchio)。
|
||
用法:env PYTHONPATH= LD_LIBRARY_PATH= $RETARGET_PYTHON scripts/hug_to_l20.py \
|
||
--npz data/capture_ready/hug_jar.npz --top 3 --side right [--check-frame]
|
||
做法与 $RETARGETING_ROOT/l20/retarget_l20_v2.py 一致:掌基(z=腕→中指根, y=食指根−小指根)对齐 + 逐骨方向按机器人骨长迁移
|
||
+ dex-retargeting position IK(21 点);腕姿态 R_wb = B_human @ B_robot^T。
|
||
"""
|
||
import argparse, json, os, sys
|
||
from pathlib import Path
|
||
from project_paths import configured_path
|
||
import numpy as np, yaml
|
||
from scipy.spatial.transform import Rotation as Rot
|
||
|
||
ROOT = Path(__file__).resolve().parents[1]
|
||
L20DIR = configured_path('RETARGETING_ROOT', 'external/retargeting')
|
||
sys.path.insert(0, str(L20DIR / "l20"))
|
||
from retarget_l20_v2 import palm_basis, _unit, CHAINS # noqa: E402
|
||
|
||
TABLE_TOP = 0.755
|
||
HIGH_PEN = 3.0 # 惩罚腕远高于 click(=物体中部):偏好"握在中部"的侧握而非"抓瓶顶"的俯压;高瓶效果明显、矮物体各候选罚值相近不受影响
|
||
|
||
|
||
def build_retargeter(side="right", config_path=None):
|
||
cfg_file = Path(config_path).resolve() if config_path else L20DIR / f"config/l20/l20_{side}_position.yml"
|
||
from dex_retargeting.retargeting_config import RetargetingConfig
|
||
cfg = yaml.safe_load(open(cfg_file))["retargeting"]
|
||
urdf = Path(cfg["urdf_path"]).expanduser()
|
||
if not urdf.is_absolute():
|
||
urdf = (ROOT / urdf) if cfg_file.is_relative_to(ROOT) else (cfg_file.parent / urdf)
|
||
cfg["urdf_path"] = str(urdf)
|
||
rt = RetargetingConfig.from_dict(cfg).build()
|
||
opt = rt.optimizer; robot = opt.robot
|
||
link_ids = [robot.get_link_index(n) for n in cfg["target_link_names"]]
|
||
def fk_points(q):
|
||
robot.compute_forward_kinematics(q)
|
||
return np.stack([robot.get_link_pose(i)[:3, 3] for i in link_ids])
|
||
neutral = fk_points(np.zeros(robot.dof))
|
||
return rt, robot, fk_points, neutral, list(opt.target_joint_names), list(robot.dof_joint_names)
|
||
|
||
|
||
def build_target(P, neutral, Br, L):
|
||
Bh = palm_basis(P)
|
||
align = Bh @ Br.T
|
||
A = (P - P[0]) @ align
|
||
T = neutral.copy()
|
||
for ch in CHAINS:
|
||
for a, b in zip(ch[:-1], ch[1:]):
|
||
d = _unit(A[b] - A[a], fallback=_unit(neutral[b] - neutral[a]))
|
||
T[b] = T[a] + L[(a, b)] * d
|
||
# 拇指用位置目标而不是方向迁移:L20 拇指根在掌侧外 4.8 cm,方向迁移会把拇指尖推到人手位置外 5 cm(穿桌)
|
||
T[[1, 2, 3, 4]] = A[[1, 2, 3, 4]]
|
||
return T, Bh @ Br.T
|
||
|
||
|
||
def main():
|
||
ap = argparse.ArgumentParser()
|
||
ap.add_argument("--npz", required=True); ap.add_argument("--top", type=int, default=3)
|
||
ap.add_argument("--side", choices=["left", "right"], default="right", help="左手或右手")
|
||
ap.add_argument("--check-frame", action="store_true"); ap.add_argument("--min-wrist-height", type=float, default=0.03)
|
||
ap.add_argument("--table-top", type=float, default=TABLE_TOP, help="桌面高度(与输入 world/base_link 坐标系一致,仿真默认 0.755;真机 base_link 传约 -0.265)")
|
||
ap.add_argument("--scale", default="auto", help="人手→机器手尺寸放大倍数(绕指尖质心放大人手 landmark):auto=机器人/人手 中指链骨长比;或给定数值;1=不放大")
|
||
ap.add_argument("--anchor", choices=["closure", "tips", "tips4", "palm", "wrist"], default="closure",
|
||
help="人手→机器手的位置锚点:tips=五指尖质心重合(默认,保留接触几何);palm=四指 MCP 中心重合;wrist=腕对腕(机器手比人手大 1.5 倍,指尖会越过物体)")
|
||
ap.add_argument("--mirror-human-hand", action="store_true", help="Reflect HUG right-hand landmarks about the fingertip-centroid palm plane for left-hand retargeting (offline candidate only)")
|
||
ap.add_argument("--config", type=Path, help="Explicit retargeting config matching the assembled hand")
|
||
a = ap.parse_args()
|
||
rt, robot, fk_points, neutral, tgt_names, dof_names = build_retargeter(side=a.side, config_path=a.config)
|
||
Br = palm_basis(neutral)
|
||
L = {(x, y): float(np.linalg.norm(neutral[y] - neutral[x])) for ch in CHAINS for x, y in zip(ch[:-1], ch[1:])}
|
||
table_top = float(a.table_top)
|
||
if a.check_frame:
|
||
print("URDF 零位 landmark(hand_base_link 系):")
|
||
names = yaml.safe_load(open(a.config or L20DIR / f"config/l20/l20_{a.side}_position.yml"))["retargeting"]["target_link_names"]
|
||
for n, p in zip(names, neutral): print(f" {n:20s} {p.round(3).tolist()}")
|
||
print("robot palm basis (cols x,y,z):\n", Br.round(3))
|
||
PRE_FRAC = 0.35; THUMB_PRE_FRAC = 0.0
|
||
# 机器手"闭合中心":各屈曲关节到 80% 闭合时的五指尖质心(手基座系)——合指时物体应位于此处
|
||
CLOSED = {"thumb_cmc_yaw": 1.2, "thumb_cmc_roll": 0.6, "thumb_cmc_pitch": 0.6, "thumb_mcp": 1.0,
|
||
"index_mcp_pitch": 1.1, "index_pip": 1.5, "middle_mcp_pitch": 1.1, "middle_pip": 1.5,
|
||
"ring_mcp_pitch": 1.1, "ring_pip": 1.5, "pinky_mcp_pitch": 1.1, "pinky_pip": 1.5}
|
||
qc = np.zeros(robot.dof)
|
||
for k, v in CLOSED.items(): qc[dof_names.index(k)] = 0.8 * v
|
||
closure_c = fk_points(qc)[[4, 8, 12, 16, 20]].mean(0)
|
||
print(f"closure center (hand frame) = {closure_c.round(3).tolist()}")
|
||
D = np.load(a.npz)
|
||
LW = D["landmarks_world"]; click = D["click_world"]; N = LW.shape[0]
|
||
chain = [(0, 9), (9, 10), (10, 11), (11, 12)]
|
||
robot_len = sum(np.linalg.norm(neutral[b] - neutral[a]) for a, b in chain)
|
||
cands = []
|
||
for i in range(N):
|
||
P0 = LW[i].copy()
|
||
if a.mirror_human_hand:
|
||
center = P0[[4, 8, 12, 16, 20]].mean(0)
|
||
lateral = palm_basis(P0)[:, 1]
|
||
reflection = np.eye(3) - 2 * np.outer(lateral, lateral)
|
||
P0 = center + (P0 - center) @ reflection.T
|
||
human_len = sum(np.linalg.norm(P0[b] - P0[a]) for a, b in chain)
|
||
s_ = robot_len / human_len if a.scale == "auto" else float(a.scale)
|
||
c0 = P0[[4, 8, 12, 16, 20]].mean(0)
|
||
P = c0 + s_ * (P0 - c0) # 绕指尖质心放大:接触几何相对物体不变,掌/腕按机器手尺寸外推
|
||
if i == 0: print(f"scale = {s_:.2f} (robot 腕→中指尖骨长 {robot_len*100:.1f} cm / human {human_len*100:.1f} cm)")
|
||
T, R_wb = build_target(P, neutral, Br, L)
|
||
# 多起点 IK(nlopt 局部解,拇指常卡在张开分支):零位 / 拇指对掌 / 半握,取指尖误差最小者
|
||
starts = [np.zeros(16), np.array([0.8, 1.2, 0.3, 0.3] + [0, 0.5, 0.5] * 4), np.array([0.4, 0.6, 0.2, 0.2] + [0, 0.3, 0.3] * 4)]
|
||
best = None
|
||
for q0 in starts:
|
||
rt.last_qpos = q0.astype(np.float32)
|
||
qc = np.asarray(rt.retarget(T), np.float64); fkc = fk_points(qc)
|
||
e = np.linalg.norm(fkc[[4, 8, 12, 16, 20]] - T[[4, 8, 12, 16, 20]], axis=1).mean()
|
||
if best is None or e < best[0]: best = (e, qc)
|
||
q = best[1]
|
||
act = {n: float(q[dof_names.index(n)]) for n in tgt_names}
|
||
fk = fk_points(q)
|
||
tip_err = np.linalg.norm(fk[[4, 8, 12, 16, 20]] - T[[4, 8, 12, 16, 20]], axis=1).mean()
|
||
# 位置锚点:让机器手的某个参考点与人手对应点在世界系重合,wrist = 该点 - R_wb @ 机器手系里的参考点
|
||
if a.anchor == "closure": # 人手指尖质心(≈被抓部位)与机器手闭合中心重合,姿态取 HUG 掌基
|
||
wrist = P0[[4, 8, 12, 16, 20]].mean(0) - R_wb @ closure_c
|
||
elif a.anchor == "tips":
|
||
wrist = P[[4, 8, 12, 16, 20]].mean(0) - R_wb @ fk[[4, 8, 12, 16, 20]].mean(0)
|
||
elif a.anchor == "tips4": # 四指尖质心(拇指可达性最差,不让它拉偏整只手)
|
||
wrist = P[[8, 12, 16, 20]].mean(0) - R_wb @ fk[[8, 12, 16, 20]].mean(0)
|
||
elif a.anchor == "palm":
|
||
wrist = P[[5, 9, 13, 17]].mean(0) - R_wb @ fk[[5, 9, 13, 17]].mean(0)
|
||
else:
|
||
wrist = P[0]
|
||
palm_n = R_wb[:, 0]; to_obj = _unit(click - wrist)
|
||
h = wrist[2] - table_top; facing = float(palm_n @ to_obj)
|
||
# 指尖世界位置(机器人 FK 经 R_wb 转世界)用于桌面穿透检查——按进给姿态算:拇指屈曲清零
|
||
q_app = q.copy()
|
||
for n in ("thumb_cmc_pitch", "thumb_mcp"): q_app[dof_names.index(n)] = 0.0
|
||
if a.anchor == "closure": # 进给预成形 = 四指屈曲到 CLOSED 的 PRE_FRAC,拇指按 CLOSED 对掌不屈曲(与 grasp_exec 一致)
|
||
for n in dof_names:
|
||
if n in CLOSED: q_app[dof_names.index(n)] = PRE_FRAC * CLOSED[n] if (n.endswith("_pitch") or n.endswith("_pip")) and not n.startswith("thumb") else 0.0
|
||
for n in ("thumb_cmc_yaw", "thumb_cmc_roll"): q_app[dof_names.index(n)] = CLOSED[n]
|
||
for n in ("thumb_cmc_pitch", "thumb_mcp"): q_app[dof_names.index(n)] = THUMB_PRE_FRAC * CLOSED[n]
|
||
fk_app = fk_points(q_app)
|
||
tips_w = (R_wb @ fk_app[[4, 8, 12, 16, 20]].T).T + wrist
|
||
min_tip_h = float(tips_w[1:, 2].min() - table_top); thumb_h = float(tips_w[0, 2] - table_top)
|
||
ok = h > a.min_wrist_height and facing > 0.2 and min_tip_h > -0.03
|
||
# 偏好"握在物体中部(click 高度)":惩罚腕高于 click——把俯压瓶顶换成中部侧握,抓高瓶更稳
|
||
score = facing - 2.0 * max(0.0, -min_tip_h) - HIGH_PEN * max(0.0, wrist[2] - click[2])
|
||
cands.append(dict(i=i, q=act, R=R_wb, wrist=wrist, tip_err=tip_err, h=h, facing=facing, min_tip_h=min_tip_h, thumb_h=thumb_h, ok=ok, score=score))
|
||
print(f" #{i:2d} wrist h={h*100:5.1f}cm facing={facing:+.2f} min_tip_h={min_tip_h*100:+5.1f}cm thumb_h={thumb_h*100:+5.1f}cm ik_tip_err={tip_err*1000:.1f}mm -> {'ok' if ok else 'drop'}")
|
||
good = sorted([c for c in cands if c["ok"]], key=lambda c: -c["score"])
|
||
print(f"{len(good)}/{N} candidates pass filters")
|
||
obj = Path(a.npz).stem.replace("hug_", ""); outdir = Path(a.npz).parent
|
||
for f in outdir.glob(f"grasp_l20_{obj}_*.json"): f.unlink()
|
||
for k, c in enumerate(good[: a.top]):
|
||
qw = Rot.from_matrix(c["R"]).as_quat() # xyzw
|
||
out = {"object": obj, "source": str(a.npz), "hand_side": a.side, "human_landmarks_mirrored": a.mirror_human_hand, "retarget_config": str(a.config.resolve()) if a.config else str(L20DIR / f"config/l20/l20_{a.side}_position.yml"), "sample": int(c["i"]), "wrist_pos": c["wrist"].round(5).tolist(),
|
||
"wrist_quat_wxyz": [float(qw[3]), float(qw[0]), float(qw[1]), float(qw[2])],
|
||
"hand_qpos": {n: round(v, 4) for n, v in c["q"].items()},
|
||
"hand_pre": {n: round(0.5 * v, 4) for n, v in c["q"].items()}, # 预成形 = 目标一半
|
||
"approach": (c["R"][:, 0]).round(4).tolist(), # 沿掌心法线进给(掌心 = +x_hand)
|
||
"anchor": a.anchor, "scale": round(float(s_), 3),
|
||
"metrics": {"ik_tip_err_mm": round(c["tip_err"] * 1000, 2), "wrist_h_cm": round(c["h"] * 100, 1), "facing": round(c["facing"], 3), "min_tip_h_cm": round(c["min_tip_h"] * 100, 1), "thumb_h_cm": round(c["thumb_h"] * 100, 1)}}
|
||
p = outdir / f"grasp_l20_{obj}_{k}.json"; p.write_text(json.dumps(out, indent=2)); print("wrote", p, out["metrics"])
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|