diff --git a/src/biomechanical_hcl_smart_simulation_platform/initial_human_mode_main.py b/src/biomechanical_hcl_smart_simulation_platform/initial_human_mode_main.py new file mode 100644 index 0000000000..fc18ba9333 --- /dev/null +++ b/src/biomechanical_hcl_smart_simulation_platform/initial_human_mode_main.py @@ -0,0 +1,518 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +initial_human_model_render.py +Render a MyoConverter MuJoCo human model (MJCF) with enhanced visuals. + +Key points (from README): +- Converted XML contains a keyframe that should be loaded at init. + Use mujoco.mj_resetDataKeyframe(model, data, 0). :contentReference[oaicite:1]{index=1} + +Features: +- Patches MJCF into a "render-friendly" version (skybox/ground/lights/quality). +- Offscreen render to PNG/GIF/MP4. +- Optional: render leg/thigh muscle "volume" as semi-transparent capsules along tendon site paths. + +Examples: + # Still image + python initial_human_model_render.py --xml path/to/model.xml --out render.png + + # Orbit GIF + python initial_human_model_render.py --xml path/to/model.xml --out render.gif --seconds 4 --orbit + + # Add muscle volume (legs by default) + python initial_human_model_render.py --xml path/to/model.xml --out render.gif --orbit --muscle-volume + +Headless: + export MUJOCO_GL=egl # (or osmesa) +""" + +from __future__ import annotations + +import argparse +import math +import os +import xml.etree.ElementTree as ET +from pathlib import Path +from typing import Dict, List, Optional, Sequence, Tuple + +import numpy as np +import mujoco + +try: + import imageio.v2 as imageio +except Exception: + imageio = None + + +# ------------------------- +# MJCF patching (for realism) +# ------------------------- + +def _abspath_if_relative(p: str, base_dir: str) -> str: + if not p: + return p + if p.startswith(("http://", "https://", "file://", "${")): + return p + if os.path.isabs(p): + return p + return os.path.abspath(os.path.join(base_dir, p)) + + +def _indent(elem: ET.Element, level: int = 0) -> None: + i = "\n" + level * " " + if len(elem): + if not elem.text or not elem.text.strip(): + elem.text = i + " " + for ch in elem: + _indent(ch, level + 1) + if not elem.tail or not elem.tail.strip(): + elem.tail = i + else: + if level and (not elem.tail or not elem.tail.strip()): + elem.tail = i + + +def patch_mjcf_for_render(in_xml: Path, out_xml: Path) -> None: + """ + - Keeps the model intact (including keyframe). + - Adds skybox, ground texture/material, floor geom, lights, and visual quality knobs. + - Makes file=... paths absolute so writing to a new folder won't break assets. + """ + base_dir = str(in_xml.parent.resolve()) + tree = ET.parse(str(in_xml)) + root = tree.getroot() + if root.tag != "mujoco": + raise ValueError(f"Not an MJCF root: {in_xml}") + + # Absolutize all file="" references + for el in root.iter(): + if "file" in el.attrib: + el.set("file", _abspath_if_relative(el.get("file", ""), base_dir)) + + asset = root.find("asset") + if asset is None: + asset = ET.SubElement(root, "asset") + + # Skybox texture (no external file) + if asset.find("./texture[@type='skybox']") is None: + ET.SubElement(asset, "texture", { + "type": "skybox", + "builtin": "gradient", + "rgb1": "0.72 0.86 1.0", + "rgb2": "0.12 0.20 0.35", + "width": "512", + "height": "512", + }) + + # Ground checker texture + material + if asset.find("./texture[@name='tex_ground']") is None: + ET.SubElement(asset, "texture", { + "name": "tex_ground", + "type": "2d", + "builtin": "checker", + "rgb1": "0.80 0.80 0.80", + "rgb2": "0.63 0.63 0.63", + "width": "512", + "height": "512", + "mark": "cross", + "markrgb": "0.35 0.35 0.35", + }) + + if asset.find("./material[@name='mat_ground']") is None: + ET.SubElement(asset, "material", { + "name": "mat_ground", + "texture": "tex_ground", + "texrepeat": "6 6", + "reflectance": "0.15", + "shininess": "0.25", + "specular": "0.35", + }) + + # Visual quality (shadows, AA, fog) + visual = root.find("visual") + if visual is None: + visual = ET.SubElement(root, "visual") + + if visual.find("quality") is None: + ET.SubElement(visual, "quality", { + "shadowsize": "4096", + "offsamples": "4", + "numslices": "28", + "numstacks": "28", + }) + + if visual.find("map") is None: + ET.SubElement(visual, "map", { + "znear": "0.01", + "zfar": "60", + "fogstart": "8", + "fogend": "25", + }) + + # Add a floor + lights into existing worldbody + worldbody = root.find("worldbody") + if worldbody is None: + worldbody = ET.SubElement(root, "worldbody") + + # Floor (avoid duplicates) + has_floor = any((g.tag == "geom" and g.get("name", "") == "floor") for g in worldbody.findall("geom")) + if not has_floor: + ET.SubElement(worldbody, "geom", { + "name": "floor", + "type": "plane", + "pos": "0 0 0", + "size": "10 10 0.1", + "material": "mat_ground", + "rgba": "1 1 1 1", + }) + + # Simple 3-point lights (avoid duplicates by name) + existing_light_names = {l.get("name", "") for l in worldbody.findall("light")} + if "key_light" not in existing_light_names: + ET.SubElement(worldbody, "light", { + "name": "key_light", + "pos": "2.5 -2.5 3.0", + "dir": "-0.6 0.6 -0.7", + "diffuse": "0.9 0.9 0.9", + "specular": "0.4 0.4 0.4", + }) + if "fill_light" not in existing_light_names: + ET.SubElement(worldbody, "light", { + "name": "fill_light", + "pos": "-2.0 2.0 2.5", + "dir": "0.5 -0.5 -0.7", + "diffuse": "0.45 0.45 0.50", + "specular": "0.15 0.15 0.15", + }) + if "rim_light" not in existing_light_names: + ET.SubElement(worldbody, "light", { + "name": "rim_light", + "pos": "0 3.5 2.8", + "dir": "0 -1 -0.6", + "diffuse": "0.35 0.35 0.40", + "specular": "0.20 0.20 0.20", + }) + + _indent(root) + out_xml.parent.mkdir(parents=True, exist_ok=True) + tree.write(str(out_xml), encoding="utf-8", xml_declaration=True) + + +# ------------------------- +# Muscle volume (capsules along tendon site path) +# ------------------------- + +DEFAULT_LEG_KEYWORDS = ( + "rect", "vasti", "vastus", "ham", "biceps", "semiten", "semimem", + "adductor", "sart", "grac", "tfl", "iliopsoas", "psoas", "iliacus", + "glut", "soleus", "gastro", "tibialis", "perone", "fibular", +) + +RGBA_REST = np.array([0.85, 0.15, 0.15, 0.40], dtype=np.float32) +RGBA_ACTIVE = np.array([1.00, 0.28, 0.28, 0.85], dtype=np.float32) + + +def _safe_name(model: mujoco.MjModel, objtype: mujoco.mjtObj, idx: int) -> str: + nm = mujoco.mj_id2name(model, objtype, idx) + return nm if nm else f"{objtype.name}_{idx}" + + +def _pick_actuators_by_keywords(model: mujoco.MjModel, keywords: Sequence[str]) -> List[int]: + keys = tuple(k.lower() for k in keywords) + picked = [] + for aid in range(model.nu): + nm = _safe_name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, aid).lower() + if any(k in nm for k in keys): + picked.append(aid) + return picked + + +def _actuator_to_tendon_id(model: mujoco.MjModel, aid: int) -> Optional[int]: + if not (hasattr(model, "actuator_trntype") and hasattr(model, "actuator_trnid")): + return None + if int(model.actuator_trntype[aid]) != int(mujoco.mjtTrn.mjTRN_TENDON): + return None + tid = int(model.actuator_trnid[aid, 0]) + if 0 <= tid < model.ntendon: + return tid + return None + + +def _tendon_sites_world(model: mujoco.MjModel, data: mujoco.MjData, tid: int) -> Optional[np.ndarray]: + # Best-effort: take SITE wrap points only + need = ("tendon_adr", "tendon_num", "wrap_type", "wrap_objid") + if not all(hasattr(model, n) for n in need): + return None + adr = int(model.tendon_adr[tid]) + num = int(model.tendon_num[tid]) + if num <= 0: + return None + + pts: List[np.ndarray] = [] + for w in range(adr, adr + num): + if int(model.wrap_type[w]) == int(mujoco.mjtWrap.mjWRAP_SITE): + sid = int(model.wrap_objid[w]) + pts.append(np.array(data.site_xpos[sid], dtype=np.float64)) + if len(pts) >= 2: + return np.stack(pts, axis=0) + return None + + +def _lerp(a: np.ndarray, b: np.ndarray, t: float) -> np.ndarray: + t = float(max(0.0, min(1.0, t))) + return (1.0 - t) * a + t * b + + +def _activation_from_ctrl(data: mujoco.MjData, aid: int) -> float: + if data.ctrl is None or aid >= data.ctrl.shape[0]: + return 0.0 + return float(max(0.0, min(1.0, float(data.ctrl[aid])))) + + +def _add_capsule(scene: mujoco.MjvScene, geom_id: int, p0: np.ndarray, p1: np.ndarray, radius: float, rgba: np.ndarray) -> None: + mujoco.mjv_initGeom( + scene.geoms[geom_id], + type=mujoco.mjtGeom.mjGEOM_CAPSULE, + size=np.zeros(3, dtype=np.float64), + pos=np.zeros(3, dtype=np.float64), + mat=np.eye(3, dtype=np.float64).flatten(), + rgba=rgba.astype(np.float32), + ) + mujoco.mjv_connector( + scene.geoms[geom_id], + type=mujoco.mjtGeom.mjGEOM_CAPSULE, + width=float(radius), + from_=p0.astype(np.float64), + to=p1.astype(np.float64), + ) + + +# ------------------------- +# Rendering +# ------------------------- + +def _init_camera(model: mujoco.MjModel, azimuth: float, elevation: float, dist_scale: float) -> mujoco.MjvCamera: + cam = mujoco.MjvCamera() + mujoco.mjv_defaultCamera(cam) + center = np.array(model.stat.center, dtype=np.float64) + extent = float(model.stat.extent) if float(model.stat.extent) > 1e-9 else 1.0 + cam.lookat[:] = center + cam.distance = max(0.05, dist_scale * extent) + cam.azimuth = float(azimuth) + cam.elevation = float(elevation) + cam.type = mujoco.mjtCamera.mjCAMERA_FREE + return cam + + +def _init_vis_options(show_tendons: bool) -> mujoco.MjvOption: + opt = mujoco.MjvOption() + mujoco.mjv_defaultOption(opt) + + # Realism-ish flags + opt.flags[mujoco.mjtVisFlag.mjVIS_SHADOW] = 1 + opt.flags[mujoco.mjtVisFlag.mjVIS_REFLECTION] = 1 + opt.flags[mujoco.mjtVisFlag.mjVIS_SKYBOX] = 1 + opt.flags[mujoco.mjtVisFlag.mjVIS_TEXTURE] = 1 + opt.flags[mujoco.mjtVisFlag.mjVIS_FOG] = 1 + opt.flags[mujoco.mjtVisFlag.mjVIS_TRANSPARENT] = 1 + + # Tendons (red lines) — can turn off when drawing muscle volume + opt.flags[mujoco.mjtVisFlag.mjVIS_TENDON] = 1 if show_tendons else 0 + return opt + + +def render( + xml_path: Path, + out_path: Path, + seconds: float, + fps: int, + width: int, + height: int, + orbit: bool, + azimuth: float, + elevation: float, + dist_scale: float, + ctrl_mode: str, + muscle_volume: bool, + muscle_keywords: Sequence[str], + base_radius: float, + bulge_gain: float, + maxgeom: int, +) -> None: + if imageio is None and out_path.suffix.lower() in (".gif", ".mp4", ".mov", ".mkv"): + raise RuntimeError("需要 imageio 导出 gif/mp4:请 `pip install imageio`。") + + model = mujoco.MjModel.from_xml_path(str(xml_path)) + data = mujoco.MjData(model) + + # Load keyframe 0 (README recommendation). :contentReference[oaicite:2]{index=2} + if model.nkey > 0: + mujoco.mj_resetDataKeyframe(model, data, 0) + else: + mujoco.mj_resetData(model, data) + mujoco.mj_forward(model, data) + + # Offscreen GL context + gl = mujoco.GLContext(width, height) + gl.make_current() + + cam = _init_camera(model, azimuth=azimuth, elevation=elevation, dist_scale=dist_scale) + opt = _init_vis_options(show_tendons=not muscle_volume) + + scn = mujoco.MjvScene(model, maxgeom=maxgeom) + con = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_100) + viewport = mujoco.MjrRect(0, 0, width, height) + + mujoco.mjr_setBuffer(mujoco.mjtFramebuffer.mjFB_OFFSCREEN, con) + mujoco.mjr_resizeOffscreen(width, height, con) + + # Precompute muscle->tendon mapping + act_to_tendon: Dict[int, int] = {} + if muscle_volume and model.nu > 0 and model.ntendon > 0: + act_ids = _pick_actuators_by_keywords(model, muscle_keywords) + for aid in act_ids: + tid = _actuator_to_tendon_id(model, aid) + if tid is not None: + act_to_tendon[aid] = tid + if not act_to_tendon: + print("[WARN] muscle-volume=ON 但未找到 tendon transmission 的 actuator;将仅使用原始渲染。") + + # output + ext = out_path.suffix.lower() + nframes = 1 if ext == ".png" else max(1, int(round(seconds * fps))) + + frames: List[np.ndarray] = [] + + phases = np.linspace(0.0, 2.0 * math.pi, num=max(1, len(act_to_tendon)), endpoint=False) + + for k in range(nframes): + t = k / max(1, fps) + + # orbit camera + if orbit: + cam.azimuth = float(azimuth + 360.0 * (k / max(1, nframes))) + + # drive controls (optional, for visible "muscle bulge") + if ctrl_mode != "none" and model.nu > 0: + data.ctrl[:] = 0.0 + if ctrl_mode == "sine": + # only drive the selected actuators (if muscle volume); otherwise drive nothing + for j, aid in enumerate(act_to_tendon.keys()): + data.ctrl[aid] = 0.5 + 0.5 * math.sin(2.0 * math.pi * 0.8 * t + float(phases[j])) + + mujoco.mj_step(model, data) + + # update scene + mujoco.mjv_updateScene(model, data, opt, None, cam, mujoco.mjtCatBit.mjCAT_ALL, scn) + + # add muscle capsules after base geoms + if muscle_volume and act_to_tendon: + base_ngeom = int(scn.ngeom) + geom_id = base_ngeom + + for aid, tid in act_to_tendon.items(): + pts = _tendon_sites_world(model, data, tid) + if pts is None: + continue + + act = _activation_from_ctrl(data, aid) + radius = float(base_radius * (1.0 + bulge_gain * act)) + rgba = _lerp(RGBA_REST, RGBA_ACTIVE, act) + + for p0, p1 in zip(pts[:-1], pts[1:]): + if geom_id >= scn.maxgeom: + break + if float(np.linalg.norm(p1 - p0)) < 1e-4: + continue + _add_capsule(scn, geom_id, p0, p1, radius, rgba) + geom_id += 1 + + scn.ngeom = geom_id + + mujoco.mjr_render(viewport, scn, con) + + rgb = np.zeros((height, width, 3), dtype=np.uint8) + depth = np.zeros((height, width), dtype=np.float32) + mujoco.mjr_readPixels(rgb, depth, viewport, con) + rgb = np.flipud(rgb) # OpenGL origin bottom-left + frames.append(rgb) + + out_path.parent.mkdir(parents=True, exist_ok=True) + + if ext == ".png": + imageio.imwrite(str(out_path), frames[0]) + else: + imageio.mimsave(str(out_path), frames, fps=fps) + + gl.free() + print(f"[OK] saved: {out_path}") + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--xml", required=True, help="Path to MyoConverter MJCF xml (e.g. models/mjc/Gait2354Simbody/gait2354_cvt3.xml)") + ap.add_argument("--out", default="human_render.gif", help="Output: .png/.gif/.mp4 (gif recommended here)") + + ap.add_argument("--workdir", default="./_render_build", help="Where to write patched XML") + ap.add_argument("--no-patch", action="store_true", help="Do not patch MJCF (use original as-is)") + + ap.add_argument("--seconds", type=float, default=4.0) + ap.add_argument("--fps", type=int, default=30) + ap.add_argument("--width", type=int, default=1280) + ap.add_argument("--height", type=int, default=720) + + ap.add_argument("--orbit", action="store_true", help="Orbit camera around the model") + ap.add_argument("--azimuth", type=float, default=95.0) + ap.add_argument("--elevation", type=float, default=-18.0) + ap.add_argument("--dist-scale", type=float, default=2.0) + + ap.add_argument("--ctrl-mode", choices=["none", "sine"], default="sine", + help="Control signal for visualizing muscle bulge (sine is just a demo)") + + ap.add_argument("--muscle-volume", action="store_true", + help="Draw semi-transparent muscle volume capsules along tendon site path (leg keywords by default).") + ap.add_argument("--muscle-keyword", action="append", default=None, + help="Add keyword for selecting actuators (repeatable). If not set, uses a leg/thigh keyword set.") + ap.add_argument("--base-radius", type=float, default=0.010, help="Base capsule radius (m)") + ap.add_argument("--bulge-gain", type=float, default=1.25, help="Radius gain by activation") + ap.add_argument("--maxgeom", type=int, default=20000, help="Scene maxgeom budget") + + args = ap.parse_args() + + src_xml = Path(args.xml).resolve() + out_path = Path(args.out).resolve() + workdir = Path(args.workdir).resolve() + workdir.mkdir(parents=True, exist_ok=True) + + if args.no_patch: + patched_xml = src_xml + else: + patched_xml = workdir / (src_xml.stem + "_render.xml") + patch_mjcf_for_render(src_xml, patched_xml) + + keywords = args.muscle_keyword if args.muscle_keyword else list(DEFAULT_LEG_KEYWORDS) + + render( + xml_path=patched_xml, + out_path=out_path, + seconds=args.seconds, + fps=args.fps, + width=args.width, + height=args.height, + orbit=args.orbit, + azimuth=args.azimuth, + elevation=args.elevation, + dist_scale=args.dist_scale, + ctrl_mode=args.ctrl_mode, + muscle_volume=args.muscle_volume, + muscle_keywords=keywords, + base_radius=args.base_radius, + bulge_gain=args.bulge_gain, + maxgeom=args.maxgeom, + ) + + +if __name__ == "__main__": + main() diff --git a/src/biomechanical_hcl_smart_simulation_platform/models_arm.py b/src/biomechanical_hcl_smart_simulation_platform/models_arm.py deleted file mode 100644 index 5252bae576..0000000000 --- a/src/biomechanical_hcl_smart_simulation_platform/models_arm.py +++ /dev/null @@ -1,184 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -""" -Render an arm model (e.g., Arm26 converted by MyoConverter) to PNG/GIF using MuJoCo Python bindings. - -Examples: - # Render a GIF (recommended) - python examples/render_arm.py --xml ./models/mjc/Arm26/arm26_cvt3.xml --out arm.gif - - # Render a still image - python examples/render_arm.py --xml ./models/mjc/Arm26/arm26_cvt3.xml --out arm.png - -Notes: - - MyoConverter converted XML often contains a keyframe that must be loaded to satisfy joint/muscle path constraints. - This script will automatically load keyframe 0 if present. (See README.) -""" - -from __future__ import annotations - -import argparse -import math -from pathlib import Path - -import numpy as np - -import mujoco -import imageio.v2 as imageio - - -def _pick_first_hinge_joint(model: mujoco.MjModel) -> int | None: - """Pick the first hinge joint as a simple 'elbow-like' joint.""" - for jid in range(model.njnt): - if int(model.jnt_type[jid]) == int(mujoco.mjtJoint.mjJNT_HINGE): - return jid - return None - - -def _joint_id_by_name(model: mujoco.MjModel, name: str) -> int | None: - try: - jid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, name) - return int(jid) if jid >= 0 else None - except Exception: - return None - - -def _setup_camera(model: mujoco.MjModel, azimuth: float, elevation: float, distance_scale: float) -> mujoco.MjvCamera: - cam = mujoco.MjvCamera() - mujoco.mjv_defaultCamera(cam) - - # A reasonable default framing: focus model center, distance proportional to extent - extent = float(model.stat.extent) if float(model.stat.extent) > 0 else 1.0 - center = np.array(model.stat.center, dtype=float) - - cam.lookat[:] = center - cam.distance = max(0.05, distance_scale * extent) - cam.azimuth = float(azimuth) - cam.elevation = float(elevation) - return cam - - -def _reset_to_keyframe_if_any(model: mujoco.MjModel, data: mujoco.MjData) -> None: - # README: converted XML contains keyframe; MuJoCo doesn't load it by default. - # Use mj_resetDataKeyframe(model, data, 0) to load keyframe 0. - if model.nkey > 0: - mujoco.mj_resetDataKeyframe(model, data, 0) - else: - mujoco.mj_resetData(model, data) - mujoco.mj_forward(model, data) - - -def render( - xml_path: Path, - out_path: Path, - width: int, - height: int, - fps: int, - seconds: float, - joint_name: str | None, - azimuth: float, - elevation: float, - distance_scale: float, -) -> None: - if not xml_path.exists(): - raise FileNotFoundError(f"XML not found: {xml_path}") - - model = mujoco.MjModel.from_xml_path(str(xml_path)) - data = mujoco.MjData(model) - - _reset_to_keyframe_if_any(model, data) - - # Choose joint - jid = _joint_id_by_name(model, joint_name) if joint_name else None - if jid is None: - jid = _pick_first_hinge_joint(model) - - if jid is None: - print("[WARN] No hinge joint found. Will just render the initial pose.") - else: - print(f"[INFO] Using joint id={jid}, name='{mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, jid)}'") - - # Joint qpos address (hinge = 1 DoF) - qadr = int(model.jnt_qposadr[jid]) if jid is not None else None - - # Joint range (if not defined, fall back) - if jid is not None: - jrange = np.array(model.jnt_range[jid], dtype=float) - if np.all(np.isfinite(jrange)) and (jrange[1] > jrange[0]): - lo, hi = float(jrange[0]), float(jrange[1]) - else: - lo, hi = -0.5, 0.5 - mid = 0.5 * (lo + hi) - amp = 0.45 * (hi - lo) - else: - lo, hi, mid, amp = 0.0, 0.0, 0.0, 0.0 - - cam = _setup_camera(model, azimuth=azimuth, elevation=elevation, distance_scale=distance_scale) - renderer = mujoco.Renderer(model, height=height, width=width) - - out_path.parent.mkdir(parents=True, exist_ok=True) - suffix = out_path.suffix.lower() - - if suffix == ".png": - # still image - renderer.update_scene(data, camera=cam) - rgb = renderer.render() - imageio.imwrite(out_path, rgb) - print(f"[OK] Saved: {out_path}") - return - - if suffix != ".gif": - raise ValueError(f"Unsupported output format '{suffix}'. Use .gif or .png") - - n_frames = max(1, int(round(seconds * fps))) - frames = [] - - for i in range(n_frames): - t = i / fps - - if jid is not None and qadr is not None: - # simple sinusoidal motion within range - angle = mid + amp * math.sin(2.0 * math.pi * (t / max(1e-6, seconds))) - data.qpos[qadr] = angle - data.qvel[:] = 0.0 - mujoco.mj_forward(model, data) - - renderer.update_scene(data, camera=cam) - rgb = renderer.render() - frames.append(rgb) - - imageio.mimsave(out_path, frames, fps=fps) - print(f"[OK] Saved: {out_path}") - - -def main(): - ap = argparse.ArgumentParser() - ap.add_argument("--xml", type=str, required=True, help="Path to MuJoCo XML (e.g. ./models/mjc/Arm26/arm26_cvt3.xml)") - ap.add_argument("--out", type=str, default="arm.gif", help="Output file: .gif or .png") - ap.add_argument("--width", type=int, default=1280) - ap.add_argument("--height", type=int, default=720) - ap.add_argument("--fps", type=int, default=30) - ap.add_argument("--seconds", type=float, default=2.0) - ap.add_argument("--joint", type=str, default=None, help="Optional joint name to animate (otherwise first hinge joint)") - ap.add_argument("--azimuth", type=float, default=110.0) - ap.add_argument("--elevation", type=float, default=-20.0) - ap.add_argument("--distance_scale", type=float, default=1.6) - - args = ap.parse_args() - render( - xml_path=Path(args.xml), - out_path=Path(args.out), - width=args.width, - height=args.height, - fps=args.fps, - seconds=args.seconds, - joint_name=args.joint, - azimuth=args.azimuth, - elevation=args.elevation, - distance_scale=args.distance_scale, - ) - - -if __name__ == "__main__": - main() diff --git a/src/biomechanical_hcl_smart_simulation_platform/models_leg.py b/src/biomechanical_hcl_smart_simulation_platform/models_leg.py deleted file mode 100644 index c6b825049b..0000000000 --- a/src/biomechanical_hcl_smart_simulation_platform/models_leg.py +++ /dev/null @@ -1,214 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -""" -Render a leg model (e.g., Leg6Dof9Musc converted by MyoConverter) to PNG/GIF using MuJoCo Python bindings. - -Examples: - # Render a GIF (recommended) - python examples/render_leg.py --xml ./models/mjc/Leg6Dof9Musc/leg6dof9musc_cvt3.xml --out leg.gif - - # Render a still image - python examples/render_leg.py --xml ./models/mjc/Leg6Dof9Musc/leg6dof9musc_cvt3.xml --out leg.png - -Notes: - - Converted XML contains a keyframe that should be loaded to satisfy joint/muscle path constraints. - MuJoCo doesn't load it by default, so we call mj_resetDataKeyframe(model, data, 0). -""" - -from __future__ import annotations - -import argparse -import math -from pathlib import Path -from typing import List, Tuple - -import numpy as np -import mujoco -import imageio.v2 as imageio - - -# ----------------------------- helpers ----------------------------- - -def reset_to_keyframe_if_any(model: mujoco.MjModel, data: mujoco.MjData) -> None: - """Load keyframe 0 if present; otherwise do a normal reset.""" - if model.nkey > 0: - mujoco.mj_resetDataKeyframe(model, data, 0) - else: - mujoco.mj_resetData(model, data) - mujoco.mj_forward(model, data) - - -def setup_camera(model: mujoco.MjModel, azimuth: float, elevation: float, distance_scale: float) -> mujoco.MjvCamera: - cam = mujoco.MjvCamera() - mujoco.mjv_defaultCamera(cam) - - extent = float(model.stat.extent) if float(model.stat.extent) > 0 else 1.0 - center = np.array(model.stat.center, dtype=float) - - cam.lookat[:] = center - cam.distance = max(0.05, distance_scale * extent) - cam.azimuth = float(azimuth) - cam.elevation = float(elevation) - return cam - - -def joint_name(model: mujoco.MjModel, jid: int) -> str: - n = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, jid) - return "" if n is None else str(n) - - -def pick_leg_joints(model: mujoco.MjModel) -> List[int]: - """ - Heuristic: - 1) Prefer hinge joints whose names contain knee/ankle/hip (case-insensitive). - 2) If none found, fall back to the first hinge joint. - """ - candidates: List[Tuple[int, int]] = [] # (priority, jid) - keys = [ - ("knee", 0), - ("ankle", 1), - ("hip", 2), - ("talus", 3), - ("subtalar", 3), - ] - - for jid in range(model.njnt): - jtype = int(model.jnt_type[jid]) - if jtype != int(mujoco.mjtJoint.mjJNT_HINGE): - continue - name = joint_name(model, jid).lower() - for k, pri in keys: - if k in name: - candidates.append((pri, jid)) - break - - if candidates: - candidates.sort(key=lambda x: x[0]) - # Keep up to 3 joints to animate (knee + ankle + hip is usually enough) - return [jid for _, jid in candidates[:3]] - - # fallback: first hinge joint - for jid in range(model.njnt): - if int(model.jnt_type[jid]) == int(mujoco.mjtJoint.mjJNT_HINGE): - return [jid] - - return [] - - -def get_range_or_default(model: mujoco.MjModel, jid: int, default_lo=-0.5, default_hi=0.5) -> Tuple[float, float]: - r = np.array(model.jnt_range[jid], dtype=float) - if np.all(np.isfinite(r)) and (r[1] > r[0]): - return float(r[0]), float(r[1]) - return float(default_lo), float(default_hi) - - -# ----------------------------- main render ----------------------------- - -def render_leg( - xml_path: Path, - out_path: Path, - width: int, - height: int, - fps: int, - seconds: float, - azimuth: float, - elevation: float, - distance_scale: float, - joint_names: List[str], -) -> None: - if not xml_path.exists(): - raise FileNotFoundError(f"XML not found: {xml_path}") - - model = mujoco.MjModel.from_xml_path(str(xml_path)) - data = mujoco.MjData(model) - - reset_to_keyframe_if_any(model, data) - - # Choose joints - jids: List[int] = [] - - # user-specified joint names (optional) - if joint_names: - for nm in joint_names: - try: - jid = int(mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, nm)) - except Exception: - jid = -1 - if jid >= 0: - jids.append(jid) - - # heuristic fallback - if not jids: - jids = pick_leg_joints(model) - - if not jids: - print("[WARN] No hinge joint found. Will just render initial pose.") - else: - print("[INFO] Animating joints:") - for jid in jids: - print(f" - id={jid:3d} name='{joint_name(model, jid)}' qposadr={int(model.jnt_qposadr[jid])}") - - cam = setup_camera(model, azimuth=azimuth, elevation=elevation, distance_scale=distance_scale) - renderer = mujoco.Renderer(model, height=height, width=width) - - out_path.parent.mkdir(parents=True, exist_ok=True) - suffix = out_path.suffix.lower() - - # still image - if suffix == ".png": - renderer.update_scene(data, camera=cam) - rgb = renderer.render() - imageio.imwrite(out_path, rgb) - print(f"[OK] Saved: {out_path}") - return - - # gif - if suffix != ".gif": - raise ValueError(f"Unsupported output format '{suffix}'. Use .gif or .png") - - n_frames = max(1, int(round(seconds * fps))) - frames = [] - - # Precompute per-joint motion params - motion = [] - for jid in jids: - qadr = int(model.jnt_qposadr[jid]) # hinge => 1 dof - lo, hi = get_range_or_default(model, jid) - mid = 0.5 * (lo + hi) - amp = 0.40 * (hi - lo) # keep margin inside limits - motion.append((qadr, mid, amp)) - - for i in range(n_frames): - t = i / fps - - # simple gait-like phase offsets between joints - for k, (qadr, mid, amp) in enumerate(motion): - phase = (k * math.pi / 3.0) # 0, 60deg, 120deg - val = mid + amp * math.sin(2.0 * math.pi * (t / max(1e-6, seconds)) + phase) - data.qpos[qadr] = val - - data.qvel[:] = 0.0 - mujoco.mj_forward(model, data) - - renderer.update_scene(data, camera=cam) - frames.append(renderer.render()) - - imageio.mimsave(out_path, frames, fps=fps) - print(f"[OK] Saved: {out_path}") - - -def main(): - ap = argparse.ArgumentParser() - ap.add_argument("--xml", type=str, required=True, - help="Path to MuJoCo XML (e.g. ./models/mjc/Leg6Dof9Musc/leg6dof9musc_cvt3.xml)") - ap.add_argument("--out", type=str, default="leg.gif", help="Output file: .gif or .png") - ap.add_argument("--width", type=int, default=1280) - ap.add_argument("--height", type=int, default=720) - ap.add_argument("--fps", type=int, default=30) - ap.add_argument("--seconds", type=float, default=2.5) - - # camera - ap.add_argument("--azimuth", type=float, default=95.0) - ap.add_argument("--elevation", type=float, default=-15.0) - ap.add_arg_ diff --git a/src/biomechanical_hcl_smart_simulation_platform/models_neck.py b/src/biomechanical_hcl_smart_simulation_platform/models_neck.py deleted file mode 100644 index 2a53f0c950..0000000000 --- a/src/biomechanical_hcl_smart_simulation_platform/models_neck.py +++ /dev/null @@ -1,177 +0,0 @@ -""" -Render a PNG image for the MyoConverter Neck6D converted model (MuJoCo XML). - -What this script does: -1) Load the converted MuJoCo model: models/mjc/Neck6D/neck6d_cvt3.xml -2) Reset and load keyframe 0 (required by MyoConverter README) -3) Step a few frames -4) Offscreen render a PNG image to output/neck6d_render.png - -Optional: -- If you pass --viewer, it will open an interactive viewer (if supported). - -Usage: - python examples/render_neck6d_png.py - python examples/render_neck6d_png.py --xml models/mjc/Neck6D/neck6d_cvt3.xml --out output/neck6d_render.png - python examples/render_neck6d_png.py --viewer -""" - -import argparse -import os -from pathlib import Path - -import mujoco -from PIL import Image - - -def ensure_parent_dir(filepath: str) -> None: - Path(filepath).parent.mkdir(parents=True, exist_ok=True) - - -def set_camera_like_default(scene, lookat=None, distance=None, azimuth=None, elevation=None): - """ - Simple camera setup helper. - If your teacher wants exactly the same viewpoint as a reference image, - you can tune these parameters. - """ - # scene is mujoco.MjvScene; camera stored in scene.camera - cam = scene.camera - - # cam.lookat is a 3-vector - if lookat is not None: - cam.lookat[:] = lookat - - # cam.distance is a float - if distance is not None: - cam.distance = float(distance) - - # cam.azimuth/elevation in degrees - if azimuth is not None: - cam.azimuth = float(azimuth) - - if elevation is not None: - cam.elevation = float(elevation) - - -def offscreen_render_png( - xml_path: str, - out_path: str, - width: int = 1400, - height: int = 1000, - steps: int = 10, - camera_lookat=(0.0, 0.0, 1.1), - camera_distance=2.2, - camera_azimuth=135.0, - camera_elevation=-15.0, -) -> None: - """ - Offscreen render a single PNG frame. - """ - if not os.path.exists(xml_path): - raise FileNotFoundError(f"MuJoCo XML not found: {xml_path}") - - # Load model and data - model = mujoco.MjModel.from_xml_path(xml_path) - data = mujoco.MjData(model) - - # IMPORTANT: Load keyframe 0 (per README) - mujoco.mj_resetDataKeyframe(model, data, 0) - - # Step a few frames to settle (optional but often makes rendering stable) - for _ in range(max(0, int(steps))): - mujoco.mj_step(model, data) - - # Create renderer - renderer = mujoco.Renderer(model, width=width, height=height) - - # Update scene (so camera edits apply) - renderer.update_scene(data) - - # Adjust camera to a reasonable view (tune to match your reference) - set_camera_like_default( - renderer.scene, - lookat=camera_lookat, - distance=camera_distance, - azimuth=camera_azimuth, - elevation=camera_elevation, - ) - - # Render - img = renderer.render() - - # Save - ensure_parent_dir(out_path) - Image.fromarray(img).save(out_path) - print(f"[OK] Saved render to: {out_path}") - - -def optional_viewer(xml_path: str) -> None: - """ - Launch an interactive viewer (requires a working OpenGL context). - """ - import time - import mujoco.viewer - - model = mujoco.MjModel.from_xml_path(xml_path) - data = mujoco.MjData(model) - mujoco.mj_resetDataKeyframe(model, data, 0) - - with mujoco.viewer.launch_passive(model, data) as viewer: - while viewer.is_running(): - mujoco.mj_step(model, data) - viewer.sync() - time.sleep(0.01) - - -def main(): - parser = argparse.ArgumentParser(description="Render Neck6D MuJoCo model to a PNG.") - parser.add_argument( - "--xml", - type=str, - default="models/mjc/Neck6D/neck6d_cvt3.xml", - help="Path to MuJoCo XML model (default: models/mjc/Neck6D/neck6d_cvt3.xml)", - ) - parser.add_argument( - "--out", - type=str, - default="output/neck6d_render.png", - help="Output PNG path (default: output/neck6d_render.png)", - ) - parser.add_argument("--width", type=int, default=1400, help="Image width") - parser.add_argument("--height", type=int, default=1000, help="Image height") - parser.add_argument("--steps", type=int, default=10, help="Simulation steps before capture") - - # Camera parameters (easy to justify + easy for老师复现) - parser.add_argument("--lookat-x", type=float, default=0.0) - parser.add_argument("--lookat-y", type=float, default=0.0) - parser.add_argument("--lookat-z", type=float, default=1.1) - parser.add_argument("--distance", type=float, default=2.2) - parser.add_argument("--azimuth", type=float, default=135.0) - parser.add_argument("--elevation", type=float, default=-15.0) - - parser.add_argument( - "--viewer", - action="store_true", - help="Open interactive viewer after rendering (optional).", - ) - - args = parser.parse_args() - - offscreen_render_png( - xml_path=args.xml, - out_path=args.out, - width=args.width, - height=args.height, - steps=args.steps, - camera_lookat=(args.lookat_x, args.lookat_y, args.lookat_z), - camera_distance=args.distance, - camera_azimuth=args.azimuth, - camera_elevation=args.elevation, - ) - - if args.viewer: - optional_viewer(args.xml) - - -if __name__ == "__main__": - main()