197 lines
8.1 KiB
Python
197 lines
8.1 KiB
Python
"""Inspect the controller arm + measured adapter + left hand, without hardware.
|
|
|
|
This is a kinematic viewer: mj_forward only, never mj_step. The saved arm
|
|
snapshot is historical; hand joint angles are nominal zeros, not live readings.
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
from collections import deque
|
|
import json
|
|
import os
|
|
from pathlib import Path
|
|
import subprocess
|
|
import sys
|
|
import time
|
|
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
SCENE = ROOT / "scene/real_assembly.xml"
|
|
INFO = ROOT / "scene/real_assembly.json"
|
|
POSES = ("snapshot", "zero", "elbow90")
|
|
CAMERAS = ("overview", "left_hand_close", "reference")
|
|
|
|
|
|
def arguments(argv=None):
|
|
parser = argparse.ArgumentParser(description=__doc__)
|
|
parser.add_argument("--pose", choices=POSES, default="elbow90")
|
|
parser.add_argument("--camera", choices=CAMERAS)
|
|
parser.add_argument("--check", action="store_true", help="无窗口检查装配和姿态")
|
|
parser.add_argument("--render", type=Path, metavar="PNG", help="离线渲染图片并退出")
|
|
parser.add_argument("--width", type=int, default=1280)
|
|
parser.add_argument("--height", type=int, default=960)
|
|
parser.add_argument("--show-table", action="store_true", help="显示估计桌面参考区域")
|
|
parser.add_argument("--no-build", action="store_true", help="使用现有场景,不重新构建")
|
|
return parser.parse_args(argv)
|
|
|
|
|
|
def build_scene(no_build=False):
|
|
if no_build:
|
|
if not SCENE.is_file() or not INFO.is_file():
|
|
raise FileNotFoundError("场景尚未构建;去掉 --no-build 后重试")
|
|
return
|
|
subprocess.run(
|
|
[sys.executable, str(ROOT / "scene/build_real_assembly.py")],
|
|
cwd=ROOT,
|
|
check=True,
|
|
)
|
|
|
|
|
|
def select_pose(mujoco, model, data, pose):
|
|
key_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_KEY, pose)
|
|
if key_id < 0:
|
|
raise ValueError(f"场景缺少姿态 {pose!r},请重新构建")
|
|
mujoco.mj_resetDataKeyframe(model, data, key_id)
|
|
mujoco.mj_forward(model, data)
|
|
|
|
|
|
def check_scene(mujoco, model, data, info, pose):
|
|
body_name = info["hands"]["left"]["base_body"]
|
|
hand = data.body(body_name)
|
|
rotation = hand.xmat.reshape(3, 3)
|
|
arm_q = {}
|
|
for side in ("left", "right"):
|
|
arm_q[side] = {
|
|
name: float(data.qpos[model.joint(name).qposadr[0]])
|
|
for name in info["arms"][side]["joints"]
|
|
}
|
|
snapshot_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_KEY, "snapshot")
|
|
out_of_range = []
|
|
for side in ("left", "right"):
|
|
for name in info["arms"][side]["joints"]:
|
|
joint = model.joint(name)
|
|
value = float(model.key_qpos[snapshot_id, joint.qposadr[0]])
|
|
low, high = (float(x) for x in joint.range)
|
|
if joint.limited[0] and not low - 1e-9 <= value <= high + 1e-9:
|
|
out_of_range.append({"joint": name, "q_rad": value, "range_rad": [low, high]})
|
|
return {
|
|
"scene": str(SCENE),
|
|
"pose": pose,
|
|
"mode": "kinematic_only_no_hardware_no_dynamics",
|
|
"snapshot_file": info.get("snapshot_file"),
|
|
"snapshot_is_live": False,
|
|
"hand_pose": "nominal_zero_not_measured",
|
|
"nq": model.nq,
|
|
"arm_q_rad": arm_q,
|
|
"left_hand_origin_world_m": hand.xpos.tolist(),
|
|
"left_palm_normal_world": rotation[:, 0].tolist(),
|
|
"left_pinky_direction_world": rotation[:, 1].tolist(),
|
|
"left_fingers_axis_world": rotation[:, 2].tolist(),
|
|
"snapshot_outside_model_joint_limits": out_of_range,
|
|
}
|
|
|
|
|
|
def render_image(mujoco, model, data, args, camera):
|
|
import cv2
|
|
|
|
model.vis.global_.offwidth = max(model.vis.global_.offwidth, args.width)
|
|
model.vis.global_.offheight = max(model.vis.global_.offheight, args.height)
|
|
options = mujoco.MjvOption()
|
|
options.geomgroup[2] = 0
|
|
options.geomgroup[3] = int(args.show_table)
|
|
options.sitegroup[4] = 0
|
|
with mujoco.Renderer(model, height=args.height, width=args.width) as renderer:
|
|
renderer.update_scene(data, camera=camera, scene_option=options)
|
|
pixels = renderer.render()
|
|
path = args.render.expanduser().resolve()
|
|
path.parent.mkdir(parents=True, exist_ok=True)
|
|
if not cv2.imwrite(str(path), cv2.cvtColor(pixels, cv2.COLOR_RGB2BGR)):
|
|
raise OSError(f"无法写入渲染图:{path}")
|
|
print(path)
|
|
|
|
|
|
def interactive(mujoco, model, data, args, camera):
|
|
import mujoco.viewer
|
|
|
|
commands = deque()
|
|
|
|
def on_key(key):
|
|
commands.append(key)
|
|
|
|
def set_camera(viewer, name):
|
|
camera_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_CAMERA, name)
|
|
if camera_id < 0:
|
|
raise ValueError(f"场景没有相机 {name}")
|
|
viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FIXED
|
|
viewer.cam.fixedcamid = camera_id
|
|
|
|
print("纯仿真检查:不连接真机,不发关节命令,不运行物理积分。", flush=True)
|
|
print("1=历史实机快照,2=全零位,3=仅左肘 J4 +90°;H=手部,V=全景,R=参考视角。", flush=True)
|
|
print("F=可旋转自由视角,T=显示/隐藏桌面参考;可在左侧 Joints 调整仿真关节。", flush=True)
|
|
print("手指为模型名义零位,未读取实机手指;桌面只作参考,不代表碰撞安全通过。", flush=True)
|
|
with mujoco.viewer.launch_passive(model, data, key_callback=on_key) as viewer:
|
|
with viewer.lock():
|
|
set_camera(viewer, camera)
|
|
viewer.opt.geomgroup[2] = 0
|
|
viewer.opt.geomgroup[3] = int(args.show_table)
|
|
viewer.opt.sitegroup[4] = 0
|
|
while viewer.is_running():
|
|
with viewer.lock():
|
|
while commands:
|
|
key = commands.popleft()
|
|
if key in (ord("1"), ord("2"), ord("3")):
|
|
pose = POSES[key - ord("1")]
|
|
select_pose(mujoco, model, data, pose)
|
|
print(f"姿态:{pose}", flush=True)
|
|
elif key in (ord("H"), ord("h")):
|
|
set_camera(viewer, "left_hand_close")
|
|
elif key in (ord("V"), ord("v")):
|
|
set_camera(viewer, "overview")
|
|
elif key in (ord("R"), ord("r")):
|
|
set_camera(viewer, "reference")
|
|
elif key in (ord("T"), ord("t")):
|
|
viewer.opt.geomgroup[3] = not viewer.opt.geomgroup[3]
|
|
elif key in (ord("F"), ord("f")):
|
|
mujoco.mjv_defaultFreeCamera(model, viewer.cam)
|
|
viewer.cam.lookat[:] = (0.25, 0.0, -0.20)
|
|
viewer.cam.distance = 1.35
|
|
viewer.cam.azimuth = 145
|
|
viewer.cam.elevation = -20
|
|
# Preserve user-edited qpos; do not reset the selected pose each frame.
|
|
mujoco.mj_forward(model, data)
|
|
viewer.sync()
|
|
time.sleep(1.0 / 60.0)
|
|
|
|
|
|
def main(argv=None):
|
|
args = arguments(argv)
|
|
if args.width <= 0 or args.height <= 0:
|
|
raise ValueError("渲染图像宽高必须大于零")
|
|
if args.render:
|
|
# Select before importing mujoco; an explicitly chosen backend still wins.
|
|
os.environ.setdefault("MUJOCO_GL", "glfw")
|
|
import mujoco
|
|
|
|
build_scene(args.no_build)
|
|
model = mujoco.MjModel.from_xml_path(str(SCENE))
|
|
data = mujoco.MjData(model)
|
|
info = json.loads(INFO.read_text())
|
|
select_pose(mujoco, model, data, args.pose)
|
|
report = check_scene(mujoco, model, data, info, args.pose)
|
|
if args.check:
|
|
print(json.dumps(report, ensure_ascii=False, indent=2))
|
|
else:
|
|
print(f"场景:{SCENE}\n历史快照:{report['snapshot_file']}")
|
|
if report["snapshot_outside_model_joint_limits"]:
|
|
print("注意:历史快照有关节超出下载模型限位,保留实测值、不裁剪:")
|
|
print(json.dumps(report["snapshot_outside_model_joint_limits"], ensure_ascii=False))
|
|
camera = args.camera or ("reference" if args.pose != "snapshot" else "overview")
|
|
if args.render:
|
|
render_image(mujoco, model, data, args, camera)
|
|
elif not args.check:
|
|
interactive(mujoco, model, data, args, camera)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|