Files

140 lines
7.0 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.
"""用 mink 解双臂 ready 姿态:手悬在桌面上方两侧,掌心朝下、指尖前倾,且不挡胸口相机。
用法:.venv/bin/python scripts/ready_pose.py [--write] # --write 把结果写回 scene/a7l20_scene.json 的 ready 字段
"""
from __future__ import annotations
import argparse, json, sys
from pathlib import Path
import mujoco, numpy as np, mink
ROOT = Path(__file__).resolve().parents[1]
XML, META = ROOT / "scene/a7l20_scene.xml", ROOT / "scene/a7l20_scene.json"
# 手基座(hand_base_link)目标:世界坐标位置 + 姿态(掌心 = +x_hand,指向 = +z_hand)
READY = {
"right": {"pos": (0.26, -0.45, 0.93), "tilt_deg": 30.0},
"left": {"pos": (0.26, 0.45, 0.93), "tilt_deg": 30.0},
}
def target_R(tilt_deg: float) -> np.ndarray:
"""指向 +x 向前并向下倾 tilt,掌心朝下(-z);列 = [x_hand, y_hand, z_hand] 在世界系。"""
t = np.deg2rad(tilt_deg)
z = np.array([np.cos(t), 0.0, -np.sin(t)]) # 指向
x = np.array([-np.sin(t), 0.0, -np.cos(t)]) # 掌心(与 z 垂直,主要朝下)
y = np.cross(z, x)
R = np.stack([x, y, z], axis=1)
assert abs(np.linalg.det(R) - 1) < 1e-9
return R
def solve(m: mujoco.MjModel, info: dict, iters: int = 400, dt: float = 0.02) -> dict[str, np.ndarray]:
cfg = mink.Configuration(m)
tasks, targets = [], {}
for side, spec in READY.items():
body = info["hands"][side]["base_body"]
t = mink.FrameTask(body, "body", position_cost=1.0, orientation_cost=0.5, lm_damping=1e-2)
R = target_R(spec["tilt_deg"])
q = np.empty(4); mujoco.mju_mat2Quat(q, R.reshape(9))
t.set_target(mink.SE3(np.concatenate([q, np.asarray(spec["pos"], float)])))
tasks.append(t); targets[side] = (np.asarray(spec["pos"]), R)
q_prior = m.qpos0.copy()
for s in READY:
for jn, q in zip(info["arms"][s]["joints"], info["arms"][s]["ready"]):
q_prior[m.jnt_qposadr[m.joint(jn).id]] = q
posture = mink.PostureTask(m, cost=5e-3); posture.set_target(q_prior); tasks.append(posture)
cfg.update(q_prior.copy())
limits = [mink.ConfigurationLimit(m)]
arm_j = {s: [m.joint(n).id for n in info["arms"][s]["joints"]] for s in READY}
for k in range(iters):
v = mink.solve_ik(cfg, tasks, dt, "daqp", damping=1e-3, limits=limits)
# 只让臂动:手指/物体自由度速度清零
mask = np.zeros(m.nv)
for s in READY:
for jid in arm_j[s]:
mask[m.jnt_dofadr[jid]] = 1.0
cfg.integrate_inplace(v * mask, dt)
if k % 100 == 0 or k == iters - 1:
errs = [np.linalg.norm(t.compute_error(cfg)) for t in tasks[:2]]
print(f" iter {k:3d} err right={errs[0]:.4f} left={errs[1]:.4f}")
out = {}
for s in READY:
out[s] = np.array([cfg.q[m.jnt_qposadr[j]] for j in arm_j[s]])
return out, cfg.q.copy(), targets
def check(m: mujoco.MjModel, info: dict, qarm: dict, targets: dict, steps: int = 2000) -> bool:
d = mujoco.MjData(m); mujoco.mj_resetData(m, d)
for s in READY:
for jn, an, q in zip(info["arms"][s]["joints"], info["arms"][s]["actuators"], qarm[s]):
d.qpos[m.jnt_qposadr[m.joint(jn).id]] = q; d.ctrl[m.actuator(an).id] = q
mujoco.mj_forward(m, d)
ok = True
for s in READY:
bid = m.body(info["hands"][s]["base_body"]).id
p, R = d.xpos[bid], d.xmat[bid].reshape(3, 3)
pe = np.linalg.norm(p - targets[s][0]); ang = np.rad2deg(np.arccos(np.clip((np.trace(R.T @ targets[s][1]) - 1) / 2, -1, 1)))
tips = np.array([d.xpos[m.body(t).id] for t in info["hands"][s]["tip_bodies"]])
print(f" {s}: base={np.round(p,3)} pos_err={pe*1000:.1f}mm ang_err={ang:.2f}deg tips z∈[{tips[:,2].min():.3f},{tips[:,2].max():.3f}] x∈[{tips[:,0].min():.3f},{tips[:,0].max():.3f}]")
margins = [min(q - m.joint(jn).range[0], m.joint(jn).range[1] - q) for jn, q in zip(info["arms"][s]["joints"], qarm[s])]
print(f" q={np.round(qarm[s],3).tolist()} min limit margin={np.rad2deg(min(margins)):.1f}deg (J{int(np.argmin(margins))+1})")
ok &= min(margins) > np.deg2rad(10)
ok &= pe < 0.005 and ang < 2.0
# 静态接触:臂/手 与 桌/物体/躯干/另一臂 不该碰
bad = {}
for i in range(d.ncon):
c = d.contact[i]
b1 = m.body(m.geom_bodyid[c.geom1]).name; b2 = m.body(m.geom_bodyid[c.geom2]).name
if c.dist < -1e-4: bad[(b1, b2)] = bad.get((b1, b2), 0) + 1
if bad:
ok = False; print(" penetrating contacts:", bad)
# 动态沉降
for _ in range(steps): mujoco.mj_step(m, d)
drift = max(abs(d.qpos[m.jnt_qposadr[m.joint(jn).id]] - q) for s in READY for jn, q in zip(info["arms"][s]["joints"], qarm[s]))
print(f" after {steps} steps: max|qvel|={np.abs(d.qvel).max():.4f} arm drift={np.rad2deg(drift):.2f}deg ncon={d.ncon}")
d0 = mujoco.MjData(m); mujoco.mj_resetData(m, d0)
for _ in range(steps): mujoco.mj_step(m, d0)
for o in info["objects"]:
z, z0 = d.xpos[m.body(o["body"]).id][2], d0.xpos[m.body(o["body"]).id][2]
if abs(z - z0) > 0.005: ok = False; print(f" object {o['name']} moved: z={z:.3f} (baseline {z0:.3f})")
ok &= np.abs(d.qvel).max() < 0.05 and np.rad2deg(drift) < 1.0
return ok
def render(m, info, qarm):
import os, cv2
os.environ.setdefault("MUJOCO_GL", "glfw"); os.environ.setdefault("DISPLAY", ":1")
d = mujoco.MjData(m); mujoco.mj_resetData(m, d)
for s in READY:
for jn, an, q in zip(info["arms"][s]["joints"], info["arms"][s]["actuators"], qarm[s]):
d.qpos[m.jnt_qposadr[m.joint(jn).id]] = q; d.ctrl[m.actuator(an).id] = q
for _ in range(500): mujoco.mj_step(m, d)
r = mujoco.Renderer(m, 480, 848)
for cam in ("front", "side", "chest_cam"):
r.update_scene(d, camera=cam); img = r.render()
cv2.imwrite(str(ROOT / f"out/ready_{cam}.png"), cv2.cvtColor(img, cv2.COLOR_RGB2BGR))
print("saved out/ready_{front,side,chest_cam}.png")
def main():
ap = argparse.ArgumentParser(); ap.add_argument("--write", action="store_true"); ap.add_argument("--render", action="store_true"); args = ap.parse_args()
m = mujoco.MjModel.from_xml_path(str(XML)); info = json.loads(META.read_text())
print("solving ready pose with mink ...")
qarm, _, targets = solve(m, info)
print("checking ...")
ok = check(m, info, qarm, targets)
print("RESULT:", "OK" if ok else "FAIL")
if args.render:
render(m, info, qarm)
if args.write and ok:
info["ready"] = {"arm_qpos": {s: qarm[s].tolist() for s in READY}, "hand_ctrl": 0.0,
"targets": {s: {"pos": list(READY[s]["pos"]), "tilt_deg": READY[s]["tilt_deg"]} for s in READY},
"note": "mink FrameTask 解的双臂 ready:手基座悬在桌面上方两侧,掌心朝下、指尖前倾 30°,不进胸口相机视野"}
META.write_text(json.dumps(info, indent=2, ensure_ascii=False), encoding="utf-8")
print("wrote ready ->", META)
sys.exit(0 if ok else 1)
if __name__ == "__main__":
main()