140 lines
7.0 KiB
Python
140 lines
7.0 KiB
Python
"""用 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()
|