Files
a7l20-hug-sim/scripts/hug_to_l20.py
T

163 lines
11 KiB
Python
Raw 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.
"""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()