From 2ee15ee430c68a3fc616e476d7ae97fed445d31b Mon Sep 17 00:00:00 2001 From: qinyiquan57-debug Date: Sun, 21 Dec 2025 12:47:15 +0800 Subject: [PATCH 1/5] =?UTF-8?q?=E8=BF=90=E8=A1=8C=E6=96=B0=E5=AD=90?= =?UTF-8?q?=E9=A1=B9=E7=9B=AE=EF=BC=8C=E4=BA=BA=E4=BD=93=E6=A8=A1=E5=9E=8B?= =?UTF-8?q?=E7=9A=84=E6=90=AD=E5=BB=BA=EF=BC=8C=E6=88=90=E5=8A=9F=E6=90=AD?= =?UTF-8?q?=E5=BB=BA=E4=BA=86=E5=85=B3=E4=BA=8Eneck=E7=9A=84=E9=83=A8?= =?UTF-8?q?=E5=88=86=EF=BC=8C=E5=B9=B6=E7=94=9F=E6=88=90=E5=8A=A8=E5=9B=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../models_neck.py | 177 ++++++++++++++++++ 1 file changed, 177 insertions(+) create mode 100644 src/biomechanical_hcl_smart_simulation_platform/models_neck.py diff --git a/src/biomechanical_hcl_smart_simulation_platform/models_neck.py b/src/biomechanical_hcl_smart_simulation_platform/models_neck.py new file mode 100644 index 0000000000..2a53f0c950 --- /dev/null +++ b/src/biomechanical_hcl_smart_simulation_platform/models_neck.py @@ -0,0 +1,177 @@ +""" +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() From 8333c74300115745ee515f473cd5db1cb2a0e57e Mon Sep 17 00:00:00 2001 From: qinyiquan57-debug Date: Sun, 21 Dec 2025 20:24:34 +0800 Subject: [PATCH 2/5] =?UTF-8?q?=E4=BA=BA=E4=BD=93=E6=A8=A1=E5=9E=8B?= =?UTF-8?q?=E7=9A=84=E6=90=AD=E5=BB=BA=EF=BC=8C=E6=88=90=E5=8A=9F=E6=90=AD?= =?UTF-8?q?=E5=BB=BA=E4=BA=86=E5=85=B3=E4=BA=8Earm=E7=9A=84=E9=83=A8?= =?UTF-8?q?=E5=88=86=EF=BC=8C=E5=B9=B6=E7=94=9F=E6=88=90=E5=8A=A8=E5=9B=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../models_arm.py | 184 ++++++++++++++++++ 1 file changed, 184 insertions(+) create mode 100644 src/biomechanical_hcl_smart_simulation_platform/models_arm.py diff --git a/src/biomechanical_hcl_smart_simulation_platform/models_arm.py b/src/biomechanical_hcl_smart_simulation_platform/models_arm.py new file mode 100644 index 0000000000..5252bae576 --- /dev/null +++ b/src/biomechanical_hcl_smart_simulation_platform/models_arm.py @@ -0,0 +1,184 @@ +#!/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() From b7e99c196d289c9d7aaacf3c2fe7a52f2ebcf439 Mon Sep 17 00:00:00 2001 From: qinyiquan57-debug Date: Sun, 21 Dec 2025 21:56:02 +0800 Subject: [PATCH 3/5] =?UTF-8?q?=E4=BA=BA=E4=BD=93=E6=A8=A1=E5=9E=8B?= =?UTF-8?q?=E7=9A=84=E6=90=AD=E5=BB=BA=EF=BC=8C=E6=88=90=E5=8A=9F=E6=90=AD?= =?UTF-8?q?=E5=BB=BA=EF=BC=8C=E5=B9=B6=E7=94=9F=E6=88=90=E5=8A=A8=E5=9B=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../models_leg.py | 214 ++++++++++++++++++ 1 file changed, 214 insertions(+) create mode 100644 src/biomechanical_hcl_smart_simulation_platform/models_leg.py diff --git a/src/biomechanical_hcl_smart_simulation_platform/models_leg.py b/src/biomechanical_hcl_smart_simulation_platform/models_leg.py new file mode 100644 index 0000000000..c6b825049b --- /dev/null +++ b/src/biomechanical_hcl_smart_simulation_platform/models_leg.py @@ -0,0 +1,214 @@ +#!/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_ From e557bf72f4f7847d7f6be132b5e961465b6cd509 Mon Sep 17 00:00:00 2001 From: qinyiquan57-debug Date: Mon, 22 Dec 2025 10:02:09 +0800 Subject: [PATCH 4/5] =?UTF-8?q?=E4=BA=BA=E4=BD=93=E6=A8=A1=E5=9E=8B?= =?UTF-8?q?=E7=9A=84=E5=88=9D=E6=AD=A5=E6=B8=B2=E6=9F=93=E6=88=90=E5=8A=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../human_model_initial_render.py | 595 ++++++++++++++++++ 1 file changed, 595 insertions(+) create mode 100644 src/biomechanical_hcl_smart_simulation_platform/human_model_initial_render.py diff --git a/src/biomechanical_hcl_smart_simulation_platform/human_model_initial_render.py b/src/biomechanical_hcl_smart_simulation_platform/human_model_initial_render.py new file mode 100644 index 0000000000..c80043dcb5 --- /dev/null +++ b/src/biomechanical_hcl_smart_simulation_platform/human_model_initial_render.py @@ -0,0 +1,595 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +""" +Render a connected (welded) leg + neck composite MJCF. + +What this script does: +1) Load leg.xml and neck.xml (original files). +2) Reset each model to keyframe(0) if present (README-style init). +3) Export namespaced copies (leg_, neck_) WITHOUT to avoid nq mismatch when composing. + Also converts all `file="..."` paths to absolute paths, so assets still load from anywhere. +4) Compute relpose for an equality/weld constraint from the keyframe poses. +5) Generate a scene MJCF that s the two namespaced parts and adds the weld. +6) Load the scene, copy qpos/qvel/act from each part’s keyframe into the composite model, then render. + +Usage: + python scripts/render_leg_neck_connected.py \ + --leg /path/to/leg.xml \ + --neck /path/to/neck.xml \ + --out /path/to/out.gif + +Notes: +- If auto-detected root body is not what you want, pass --leg_root_body / --neck_root_body. +- Output scene + temp MJCFs are written under --workdir (default: ./_build_leg_neck). +""" + +from __future__ import annotations + +import argparse +import os +import sys +import math +import shutil +import xml.etree.ElementTree as ET +from dataclasses import dataclass +from typing import Dict, List, Tuple, Optional + +import numpy as np + +# imageio is optional unless you save gif/mp4 +try: + import imageio.v2 as imageio +except Exception: + imageio = None + +try: + import mujoco +except Exception as e: + raise SystemExit( + "Cannot import `mujoco`. Please install official MuJoCo python bindings, e.g.\n" + " pip install mujoco\n" + f"Original error: {e}" + ) + + +# ------------------------- +# XML helpers +# ------------------------- + +FILE_ATTR_WHITELIST = {"file"} # attributes that should be treated as filesystem paths + + +def _indent(elem: ET.Element, level: int = 0) -> None: + """Pretty-print indentation (in-place).""" + i = "\n" + level * " " + if len(elem): + if not elem.text or not elem.text.strip(): + elem.text = i + " " + for child in elem: + _indent(child, 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 _strip_keyframes(root: ET.Element) -> None: + """Remove blocks entirely.""" + for kf in list(root.findall("keyframe")): + root.remove(kf) + + +def _collect_rename_tokens(root: ET.Element) -> Dict[str, str]: + """ + Collect all tokens that should be namespaced: + - any 'name' + - any class-related tokens ('class', 'childclass') so defaults don't collide + """ + tokens: set[str] = set() + for el in root.iter(): + for k in ("name", "class", "childclass"): + v = el.get(k) + if v and v.strip(): + tokens.add(v.strip()) + return {t: t for t in tokens} + + +def _abspath_if_relative(path: str, base_dir: str) -> str: + # ignore already-absolute or URLs or mujoco-style placeholders + if not path: + return path + if path.startswith(("http://", "https://", "file://")): + return path + if path.startswith("${"): # environment var style + return path + if os.path.isabs(path): + return path + return os.path.abspath(os.path.join(base_dir, path)) + + +def namespace_mjcf( + in_xml: str, + out_xml: str, + prefix: str, + remove_keyframe: bool = True, + absolutize_files: bool = True, +) -> Tuple[str, Dict[str, str]]: + """ + Create a namespaced copy of MJCF: + - prefixes 'name' / 'class' / 'childclass' values + - replaces references by token matching (whitespace-separated) + - optionally removes + - optionally converts all file="..." to absolute paths based on in_xml directory + + Returns: + (root_body_name_original, rename_map_old_to_new) + """ + base_dir = os.path.dirname(os.path.abspath(in_xml)) + tree = ET.parse(in_xml) + root = tree.getroot() + + if remove_keyframe: + _strip_keyframes(root) + + # Build rename map + tokens = _collect_rename_tokens(root) + rename_map: Dict[str, str] = {old: f"{prefix}{old}" for old in tokens.keys()} + + # Apply rename on attributes + for el in root.iter(): + # absolutize file paths + if absolutize_files: + for attr, val in list(el.attrib.items()): + if attr in FILE_ATTR_WHITELIST and val: + el.set(attr, _abspath_if_relative(val, base_dir)) + + # rename primary naming attrs + for k in ("name", "class", "childclass"): + v = el.get(k) + if v and v in rename_map: + el.set(k, rename_map[v]) + + # rename references by token substitution + for attr, val in list(el.attrib.items()): + # skip file paths and root model name + if attr in FILE_ATTR_WHITELIST: + continue + if el is root and attr == "model": + continue + + if not val or not val.strip(): + continue + + parts = val.split() + changed = False + for i, tok in enumerate(parts): + if tok in rename_map: + parts[i] = rename_map[tok] + changed = True + if changed: + el.set(attr, " ".join(parts)) + + _indent(root) + os.makedirs(os.path.dirname(os.path.abspath(out_xml)), exist_ok=True) + tree.write(out_xml, encoding="utf-8", xml_declaration=True) + return out_xml, rename_map + + +def detect_root_body_name(xml_path: str) -> str: + """ + Heuristic: pick the first directly under . + """ + tree = ET.parse(xml_path) + root = tree.getroot() + worldbody = root.find("worldbody") + if worldbody is None: + raise ValueError(f"No found in: {xml_path}") + + bodies = worldbody.findall("body") + if not bodies: + raise ValueError(f"No directly under in: {xml_path}") + + # prefer named body + for b in bodies: + nm = b.get("name") + if nm and nm.strip(): + return nm.strip() + # fallback + return bodies[0].get("name", "").strip() or "unnamed_root_body" + + +# ------------------------- +# MuJoCo state helpers +# ------------------------- + +@dataclass +class ModelState: + joint_qpos: Dict[str, np.ndarray] + joint_qvel: Dict[str, np.ndarray] + actuator_act: Dict[str, np.ndarray] + body_pose_world: Dict[str, Tuple[np.ndarray, np.ndarray]] # name -> (pos(3), quat(4)) + + +def _joint_qpos_len(jnt_type: int) -> int: + # mjtJoint: FREE=0, BALL=1, SLIDE=2, HINGE=3 (MuJoCo convention) + if jnt_type == mujoco.mjtJoint.mjJNT_FREE: + return 7 + if jnt_type == mujoco.mjtJoint.mjJNT_BALL: + return 4 + return 1 + + +def _joint_dof_len(jnt_type: int) -> int: + if jnt_type == mujoco.mjtJoint.mjJNT_FREE: + return 6 + if jnt_type == mujoco.mjtJoint.mjJNT_BALL: + return 3 + return 1 + + +def load_and_reset(model_xml: str) -> Tuple["mujoco.MjModel", "mujoco.MjData"]: + m = mujoco.MjModel.from_xml_path(model_xml) + d = mujoco.MjData(m) + + if m.nkey > 0: + # README-style: reset to keyframe 0 + mujoco.mj_resetDataKeyframe(m, d, 0) + mujoco.mj_forward(m, d) + else: + mujoco.mj_resetData(m, d) + d.qpos[:] = m.qpos0 + mujoco.mj_forward(m, d) + return m, d + + +def extract_state(m: "mujoco.MjModel", d: "mujoco.MjData") -> ModelState: + joint_qpos: Dict[str, np.ndarray] = {} + joint_qvel: Dict[str, np.ndarray] = {} + + # joints + for jid in range(m.njnt): + jname = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_JOINT, jid) + if not jname: + continue + jtype = int(m.jnt_type[jid]) + qpos_adr = int(m.jnt_qposadr[jid]) + dof_adr = int(m.jnt_dofadr[jid]) + + qpos_len = _joint_qpos_len(jtype) + dof_len = _joint_dof_len(jtype) + + joint_qpos[jname] = np.array(d.qpos[qpos_adr:qpos_adr + qpos_len], dtype=float) + joint_qvel[jname] = np.array(d.qvel[dof_adr:dof_adr + dof_len], dtype=float) + + # actuator activations (if any) + actuator_act: Dict[str, np.ndarray] = {} + for aid in range(m.na): + aname = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_ACTUATOR, aid) + if not aname: + continue + actadr = int(m.actuator_actadr[aid]) + actnum = int(m.actuator_actnum[aid]) + if actnum > 0: + actuator_act[aname] = np.array(d.act[actadr:actadr + actnum], dtype=float) + + # body poses + body_pose_world: Dict[str, Tuple[np.ndarray, np.ndarray]] = {} + for bid in range(m.nbody): + bname = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, bid) + if not bname: + continue + pos = np.array(d.xpos[bid], dtype=float) + quat = np.array(d.xquat[bid], dtype=float) # w x y z + body_pose_world[bname] = (pos, quat) + + return ModelState( + joint_qpos=joint_qpos, + joint_qvel=joint_qvel, + actuator_act=actuator_act, + body_pose_world=body_pose_world, + ) + + +# ------------------------- +# Quaternion math +# ------------------------- + +def quat_mul(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: + """Hamilton product. q = [w,x,y,z].""" + w1, x1, y1, z1 = q1 + w2, x2, y2, z2 = q2 + return np.array([ + w1*w2 - x1*x2 - y1*y2 - z1*z2, + w1*x2 + x1*w2 + y1*z2 - z1*y2, + w1*y2 - x1*z2 + y1*w2 + z1*x2, + w1*z2 + x1*y2 - y1*x2 + z1*w2 + ], dtype=float) + + +def quat_conj(q: np.ndarray) -> np.ndarray: + w, x, y, z = q + return np.array([w, -x, -y, -z], dtype=float) + + +def quat_norm(q: np.ndarray) -> float: + return float(np.linalg.norm(q)) + + +def quat_inv(q: np.ndarray) -> np.ndarray: + n2 = float(np.dot(q, q)) + if n2 <= 0: + return np.array([1, 0, 0, 0], dtype=float) + return quat_conj(q) / n2 + + +def quat_to_mat(q: np.ndarray) -> np.ndarray: + """Quaternion (w,x,y,z) to rotation matrix.""" + w, x, y, z = q + # normalize + n = quat_norm(q) + if n == 0: + return np.eye(3) + w, x, y, z = (q / n).tolist() + + return np.array([ + [1 - 2*(y*y + z*z), 2*(x*y - z*w), 2*(x*z + y*w)], + [ 2*(x*y + z*w), 1 - 2*(x*x + z*z), 2*(y*z - x*w)], + [ 2*(x*z - y*w), 2*(y*z + x*w), 1 - 2*(x*x + y*y)], + ], dtype=float) + + +def compute_relpose(body1_pos, body1_quat, body2_pos, body2_quat) -> Tuple[np.ndarray, np.ndarray]: + """ + relpose is pose of body2 in body1 frame: + p_rel = R1^T (p2 - p1) + q_rel = inv(q1) * q2 + """ + R1 = quat_to_mat(body1_quat) + p_rel = R1.T @ (body2_pos - body1_pos) + q_rel = quat_mul(quat_inv(body1_quat), body2_quat) + + # normalize quaternion + n = quat_norm(q_rel) + if n > 0: + q_rel = q_rel / n + else: + q_rel = np.array([1, 0, 0, 0], dtype=float) + + return p_rel, q_rel + + +# ------------------------- +# Composite scene generation +# ------------------------- + +def write_scene_xml( + scene_xml: str, + leg_part_xml: str, + neck_part_xml: str, + leg_root_body: str, + neck_root_body: str, + relpose_pos: np.ndarray, + relpose_quat: np.ndarray, +) -> None: + """ + Create a scene MJCF which includes two parts and adds an equality/weld between their root bodies. + """ + os.makedirs(os.path.dirname(os.path.abspath(scene_xml)), exist_ok=True) + + rp = list(relpose_pos.flatten()) + list(relpose_quat.flatten()) + rp_str = " ".join(f"{x:.8g}" for x in rp) + + # Keep this scene minimal; you can add cameras/lights as needed. + mjcf = f""" + + + + + + + + + + + + + + +""" + with open(scene_xml, "w", encoding="utf-8") as f: + f.write(mjcf) + + +def apply_state_to_composite( + composite_model: "mujoco.MjModel", + composite_data: "mujoco.MjData", + src_state: ModelState, + prefix: str, +) -> None: + """ + Copy qpos/qvel/act by name from a source model state into the composite model, + assuming composite names are prefixed. + """ + # joints + for jname, qpos in src_state.joint_qpos.items(): + tgt_name = prefix + jname + jid = mujoco.mj_name2id(composite_model, mujoco.mjtObj.mjOBJ_JOINT, tgt_name) + if jid < 0: + continue + jtype = int(composite_model.jnt_type[jid]) + qpos_adr = int(composite_model.jnt_qposadr[jid]) + qpos_len = _joint_qpos_len(jtype) + if len(qpos) == qpos_len: + composite_data.qpos[qpos_adr:qpos_adr + qpos_len] = qpos + + for jname, qvel in src_state.joint_qvel.items(): + tgt_name = prefix + jname + jid = mujoco.mj_name2id(composite_model, mujoco.mjtObj.mjOBJ_JOINT, tgt_name) + if jid < 0: + continue + jtype = int(composite_model.jnt_type[jid]) + dof_adr = int(composite_model.jnt_dofadr[jid]) + dof_len = _joint_dof_len(jtype) + if len(qvel) == dof_len: + composite_data.qvel[dof_adr:dof_adr + dof_len] = qvel + + # actuator activation states + for aname, act in src_state.actuator_act.items(): + tgt_name = prefix + aname + aid = mujoco.mj_name2id(composite_model, mujoco.mjtObj.mjOBJ_ACTUATOR, tgt_name) + if aid < 0: + continue + actadr = int(composite_model.actuator_actadr[aid]) + actnum = int(composite_model.actuator_actnum[aid]) + if actnum > 0 and len(act) == actnum: + composite_data.act[actadr:actadr + actnum] = act + + +# ------------------------- +# Rendering +# ------------------------- + +def render_gif( + model: "mujoco.MjModel", + data: "mujoco.MjData", + out_path: str, + width: int = 960, + height: int = 540, + fps: int = 30, + frames: int = 180, + camera: str = "cam", + settle_steps: int = 20, +) -> None: + if imageio is None: + raise SystemExit("imageio is not installed. Please `pip install imageio` to save GIF/MP4.") + + # Let constraints settle a bit + for _ in range(settle_steps): + mujoco.mj_step(model, data) + + renderer = mujoco.Renderer(model, width=width, height=height) + imgs = [] + + for _ in range(frames): + mujoco.mj_step(model, data) + renderer.update_scene(data, camera=camera) + img = renderer.render() # HxWx3 uint8 + imgs.append(img) + + os.makedirs(os.path.dirname(os.path.abspath(out_path)), exist_ok=True) + ext = os.path.splitext(out_path)[1].lower() + + if ext == ".gif": + imageio.mimsave(out_path, imgs, fps=fps) + elif ext in (".mp4", ".mkv", ".mov"): + imageio.mimsave(out_path, imgs, fps=fps) # imageio chooses writer by extension + else: + raise ValueError(f"Unsupported output format: {ext} (use .gif or .mp4)") + + +# ------------------------- +# Main +# ------------------------- + +def main() -> int: + ap = argparse.ArgumentParser() + ap.add_argument("--leg", required=True, help="Path to leg MJCF (original).") + ap.add_argument("--neck", required=True, help="Path to neck MJCF (original).") + ap.add_argument("--out", required=True, help="Output .gif/.mp4 path.") + ap.add_argument("--workdir", default="./_build_leg_neck", help="Directory for generated temp MJCFs and scene.") + ap.add_argument("--leg_prefix", default="leg_", help="Namespace prefix for leg model.") + ap.add_argument("--neck_prefix", default="neck_", help="Namespace prefix for neck model.") + + ap.add_argument("--leg_root_body", default="", help="Override: body name in leg model to weld from.") + ap.add_argument("--neck_root_body", default="", help="Override: body name in neck model to weld to.") + + ap.add_argument("--width", type=int, default=960) + ap.add_argument("--height", type=int, default=540) + ap.add_argument("--fps", type=int, default=30) + ap.add_argument("--frames", type=int, default=180) + ap.add_argument("--settle_steps", type=int, default=20) + + args = ap.parse_args() + + leg_xml = os.path.abspath(args.leg) + neck_xml = os.path.abspath(args.neck) + workdir = os.path.abspath(args.workdir) + os.makedirs(workdir, exist_ok=True) + + # 1) Detect root bodies (in ORIGINAL names) + leg_root = args.leg_root_body.strip() or detect_root_body_name(leg_xml) + neck_root = args.neck_root_body.strip() or detect_root_body_name(neck_xml) + + # 2) Load originals and reset keyframe 0 (README-style) + leg_m, leg_d = load_and_reset(leg_xml) + neck_m, neck_d = load_and_reset(neck_xml) + leg_state = extract_state(leg_m, leg_d) + neck_state = extract_state(neck_m, neck_d) + + if leg_root not in leg_state.body_pose_world: + raise SystemExit(f"Cannot find body '{leg_root}' in leg model after reset. Try --leg_root_body.") + if neck_root not in neck_state.body_pose_world: + raise SystemExit(f"Cannot find body '{neck_root}' in neck model after reset. Try --neck_root_body.") + + leg_pos, leg_quat = leg_state.body_pose_world[leg_root] + neck_pos, neck_quat = neck_state.body_pose_world[neck_root] + + # 3) Compute relpose (neck body pose in leg body frame) from keyframe poses + rel_p, rel_q = compute_relpose(leg_pos, leg_quat, neck_pos, neck_quat) + + # 4) Write namespaced, no-keyframe copies (assets file paths become absolute) + leg_ns_xml = os.path.join(workdir, "leg_ns_nokey.xml") + neck_ns_xml = os.path.join(workdir, "neck_ns_nokey.xml") + + _, leg_map = namespace_mjcf(leg_xml, leg_ns_xml, prefix=args.leg_prefix, remove_keyframe=True, absolutize_files=True) + _, neck_map = namespace_mjcf(neck_xml, neck_ns_xml, prefix=args.neck_prefix, remove_keyframe=True, absolutize_files=True) + + leg_root_ns = args.leg_prefix + leg_root + neck_root_ns = args.neck_prefix + neck_root + + # 5) Generate composite scene XML + scene_xml = os.path.join(workdir, "leg_neck_connected_scene.xml") + write_scene_xml( + scene_xml=scene_xml, + leg_part_xml=leg_ns_xml, + neck_part_xml=neck_ns_xml, + leg_root_body=leg_root_ns, + neck_root_body=neck_root_ns, + relpose_pos=rel_p, + relpose_quat=rel_q, + ) + + # 6) Load composite and apply initial state by copying from original keyframes + comp_m = mujoco.MjModel.from_xml_path(scene_xml) + comp_d = mujoco.MjData(comp_m) + + mujoco.mj_resetData(comp_m, comp_d) + apply_state_to_composite(comp_m, comp_d, leg_state, prefix=args.leg_prefix) + apply_state_to_composite(comp_m, comp_d, neck_state, prefix=args.neck_prefix) + mujoco.mj_forward(comp_m, comp_d) + + # 7) Render + render_gif( + model=comp_m, + data=comp_d, + out_path=os.path.abspath(args.out), + width=args.width, + height=args.height, + fps=args.fps, + frames=args.frames, + camera="cam", + settle_steps=args.settle_steps, + ) + + print(f"[OK] Scene: {scene_xml}") + print(f"[OK] Output: {os.path.abspath(args.out)}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) From bf6f5c8e75e81c166b33c5d81196983e01e99bfc Mon Sep 17 00:00:00 2001 From: qinyiquan57-debug Date: Mon, 22 Dec 2025 14:30:49 +0800 Subject: [PATCH 5/5] =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=BA=86=E5=85=B3?= =?UTF-8?q?=E4=BA=8E=E4=BA=BA=E4=BD=93=E6=A8=A1=E5=9E=8B=E7=9A=84=E5=A4=A7?= =?UTF-8?q?=E8=85=BF=E8=82=8C=E8=82=89=E7=9A=84=E6=B8=B2=E6=9F=93=EF=BC=8C?= =?UTF-8?q?=E4=BD=BF=E6=A8=A1=E5=9E=8B=E6=9B=B4=E5=8A=A0=E7=9C=9F=E5=AE=9E?= =?UTF-8?q?=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../enhanced_human_model.py | 297 +++++++++ .../human_model_initial_render.py | 595 ------------------ 2 files changed, 297 insertions(+), 595 deletions(-) create mode 100644 src/biomechanical_hcl_smart_simulation_platform/enhanced_human_model.py delete mode 100644 src/biomechanical_hcl_smart_simulation_platform/human_model_initial_render.py diff --git a/src/biomechanical_hcl_smart_simulation_platform/enhanced_human_model.py b/src/biomechanical_hcl_smart_simulation_platform/enhanced_human_model.py new file mode 100644 index 0000000000..b9bb6e47e1 --- /dev/null +++ b/src/biomechanical_hcl_smart_simulation_platform/enhanced_human_model.py @@ -0,0 +1,297 @@ +# preliminary_human_model_render_thigh_muscles.py +# Render MuJoCo MJCF with "thigh muscle" visualization as thick capsules, optionally exporting a GIF. + +from __future__ import annotations + +import argparse +import math +from dataclasses import dataclass +from typing import Iterable, List, Optional, Sequence, Tuple + +import numpy as np + +import mujoco + +try: + import imageio.v3 as iio +except Exception: # pragma: no cover + iio = None + + +# ----------------------------- +# Config +# ----------------------------- +DEFAULT_THIGH_KEYWORDS = ( + # Quadriceps + "rectus", "rect_fem", "rf", "vasti", "vastus", + # Hamstrings + "ham", "biceps_fem", "bf", "semiten", "semimem", + # Adductors / sartorius / gracilis / TFL + "adductor", "add", "sart", "sartorius", "grac", "gracilis", "tfl", "tensor", + # Hip flexors / glutes (often affect thigh look) + "iliopsoas", "psoas", "iliacus", "glut", "glute", +) + +RGBA_REST = np.array([0.85, 0.15, 0.15, 0.45], dtype=np.float32) # relaxed muscle (semi-transparent red) +RGBA_ACTIVE = np.array([1.00, 0.25, 0.25, 0.85], dtype=np.float32) # active muscle (brighter, less transparent) + + +@dataclass +class MuscleVizSpec: + base_radius: float = 0.010 # meters + bulge_gain: float = 1.25 # radius multiplier gain: r = base*(1 + bulge_gain*act) + max_capsules: int = 3000 # budget for extra geoms + + +# ----------------------------- +# Utilities +# ----------------------------- +def _safe_name(model: mujoco.MjModel, objtype: mujoco.mjtObj, idx: int) -> str: + name = mujoco.mj_id2name(model, objtype, idx) + return name or 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: List[int] = [] + for i in range(model.nu): # in MuJoCo, #controls == #actuators for typical models + nm = _safe_name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i).lower() + if any(k in nm for k in keys): + picked.append(i) + return picked + + +def _actuator_to_tendon_id(model: mujoco.MjModel, act_id: int) -> Optional[int]: + """ + For muscle actuators using tendon transmission, actuator_trntype should be mjTRN_TENDON + and actuator_trnid[act_id, 0] is the tendon id. + """ + if not hasattr(model, "actuator_trntype") or not hasattr(model, "actuator_trnid"): + return None + trntype = int(model.actuator_trntype[act_id]) + if trntype != int(mujoco.mjtTrn.mjTRN_TENDON): + return None + tid = int(model.actuator_trnid[act_id, 0]) + if tid < 0 or tid >= model.ntendon: + return None + return tid + + +def _tendon_path_sites_world(model: mujoco.MjModel, data: mujoco.MjData, tendon_id: int) -> Optional[np.ndarray]: + """ + Best-effort extraction of tendon path point(s) using wrap sites. + If wrap-site info isn't available, returns None. + """ + required = ("tendon_adr", "tendon_num", "wrap_type", "wrap_objid") + if not all(hasattr(model, f) for f in required): + return None + + adr = int(model.tendon_adr[tendon_id]) + num = int(model.tendon_num[tendon_id]) + if num <= 0: + return None + + pts: List[np.ndarray] = [] + for w in range(adr, adr + num): + wtype = int(model.wrap_type[w]) + if wtype == int(mujoco.mjtWrap.mjWRAP_SITE): + sid = int(model.wrap_objid[w]) + pts.append(data.site_xpos[sid].copy()) + # NOTE: other wrap types (geom/pulley) are ignored here for simplicity. + + if len(pts) >= 2: + return np.stack(pts, axis=0) + return None + + +def _normalize01(x: float) -> float: + return float(max(0.0, min(1.0, x))) + + +def _activation_from_ctrl(data: mujoco.MjData, act_id: int) -> float: + if data.ctrl is None or act_id >= data.ctrl.shape[0]: + return 0.0 + return _normalize01(float(data.ctrl[act_id])) + + +def _lerp_rgba(a: np.ndarray, b: np.ndarray, t: float) -> np.ndarray: + t = _normalize01(t) + return (1.0 - t) * a + t * b + + +def _set_default_camera(model: mujoco.MjModel, cam: mujoco.MjvCamera) -> None: + # Use model statistics if available to frame the whole model nicely. + center = np.array(model.stat.center, dtype=np.float64) + extent = float(model.stat.extent) + + cam.type = mujoco.mjtCamera.mjCAMERA_FREE + cam.lookat[:] = center + cam.distance = 2.2 * extent if extent > 1e-6 else 2.0 + cam.azimuth = 90.0 + cam.elevation = -20.0 + + +def _add_capsule(scene: mujoco.MjvScene, geom_id: int, p0: np.ndarray, p1: np.ndarray, radius: float, rgba: np.ndarray) -> None: + """ + Uses mjv_initGeom + mjv_connector to create a capsule between p0 and p1. + mjv_connector assumes mjv_initGeom has set other properties like rgba. :contentReference[oaicite:3]{index=3} + """ + 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), + ) + + +# ----------------------------- +# Offscreen render loop +# ----------------------------- +def render_offscreen_gif( + mjcf_path: str, + out_path: str, + seconds: float, + fps: int, + width: int, + height: int, + keywords: Sequence[str], + viz: MuscleVizSpec, +) -> None: + if iio is None: + raise RuntimeError("imageio 未安装:请先 `pip install imageio`(或 imageio[ffmpeg] 生成 mp4)。") + + model = mujoco.MjModel.from_xml_path(mjcf_path) + data = mujoco.MjData(model) + + # Load keyframe 0 as recommended for MyoConverter outputs. :contentReference[oaicite:4]{index=4} + try: + mujoco.mj_resetDataKeyframe(model, data, 0) + mujoco.mj_forward(model, data) + except Exception: + # If no keyframe exists, continue. + mujoco.mj_forward(model, data) + + # Pick thigh-related actuators and map to tendons + act_ids = _pick_actuators_by_keywords(model, keywords) + act_to_tendon = {a: _actuator_to_tendon_id(model, a) for a in act_ids} + act_to_tendon = {a: t for a, t in act_to_tendon.items() if t is not None} + + if not act_to_tendon: + print("[WARN] 没找到匹配关键词且使用 tendon transmission 的 actuator;将只渲染骨架/身体,不画肌肉胶囊。") + + # Create OpenGL context for offscreen rendering. :contentReference[oaicite:5]{index=5} + gl_ctx = mujoco.GLContext(width, height) + gl_ctx.make_current() + + cam = mujoco.MjvCamera() + opt = mujoco.MjvOption() + scn = mujoco.MjvScene(model, maxgeom=viz.max_capsules) # allocate lots of geom slots + con = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_100) + viewport = mujoco.MjrRect(0, 0, width, height) + + _set_default_camera(model, cam) + + # Make sure we have an offscreen buffer sized correctly + mujoco.mjr_setBuffer(mujoco.mjtFramebuffer.mjFB_OFFSCREEN, con) + mujoco.mjr_resizeOffscreen(width, height, con) + + nframes = int(math.ceil(seconds * fps)) + dt = 1.0 / fps + + frames: List[np.ndarray] = [] + + # simple “demo” control signal so you can see bulging: sine-wave activations + 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 * dt + + # drive the selected thigh muscles a bit (0..1) + if model.nu > 0: + data.ctrl[:] = 0.0 + for j, (act_id, _) in enumerate(act_to_tendon.items()): + data.ctrl[act_id] = 0.5 + 0.5 * math.sin(2.0 * math.pi * 0.8 * t + float(phases[j])) + + mujoco.mj_step(model, data) + + # Update base scene (standard pipeline). :contentReference[oaicite:6]{index=6} + mujoco.mjv_updateScene(model, data, opt, None, cam, mujoco.mjtCatBit.mjCAT_ALL, scn) + + # Append custom "muscle" capsules after the model geoms + geom_id = int(scn.ngeom) + for act_id, tendon_id in act_to_tendon.items(): + pts = _tendon_path_sites_world(model, data, tendon_id) + if pts is None: + continue + + act = _activation_from_ctrl(data, act_id) + radius = float(viz.base_radius * (1.0 + viz.bulge_gain * act)) + rgba = _lerp_rgba(RGBA_REST, RGBA_ACTIVE, act) + + for p0, p1 in zip(pts[:-1], pts[1:]): + if geom_id >= scn.maxgeom: + break + # skip tiny segments + 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) # :contentReference[oaicite:7]{index=7} + rgb = np.flipud(rgb) # OpenGL origin is bottom-left + frames.append(rgb) + + iio.imwrite(out_path, np.stack(frames, axis=0), fps=fps) + gl_ctx.free() + + print(f"[OK] saved: {out_path}") + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--mjcf", required=True, help="Path to MJCF xml (e.g. models/mjc/Leg6Dof9Musc/leg6dof9musc_cvt3.xml)") + parser.add_argument("--out", default="thigh_muscles.gif", help="Output GIF path") + parser.add_argument("--seconds", type=float, default=3.0) + parser.add_argument("--fps", type=int, default=30) + parser.add_argument("--width", type=int, default=960) + parser.add_argument("--height", type=int, default=720) + parser.add_argument("--base-radius", type=float, default=0.010) + parser.add_argument("--bulge-gain", type=float, default=1.25) + parser.add_argument("--keyword", action="append", default=None, + help="Add a muscle name keyword to match actuators (can repeat). If not set, uses a thigh keyword list.") + parser.add_argument("--maxgeom", type=int, default=4000, help="MjvScene maxgeom budget (increase if many muscles)") + + args = parser.parse_args() + + keywords = args.keyword if args.keyword else list(DEFAULT_THIGH_KEYWORDS) + viz = MuscleVizSpec(base_radius=args.base_radius, bulge_gain=args.bulge_gain, max_capsules=args.maxgeom) + + render_offscreen_gif( + mjcf_path=args.mjcf, + out_path=args.out, + seconds=args.seconds, + fps=args.fps, + width=args.width, + height=args.height, + keywords=keywords, + viz=viz, + ) + + +if __name__ == "__main__": + main() diff --git a/src/biomechanical_hcl_smart_simulation_platform/human_model_initial_render.py b/src/biomechanical_hcl_smart_simulation_platform/human_model_initial_render.py deleted file mode 100644 index c80043dcb5..0000000000 --- a/src/biomechanical_hcl_smart_simulation_platform/human_model_initial_render.py +++ /dev/null @@ -1,595 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -""" -Render a connected (welded) leg + neck composite MJCF. - -What this script does: -1) Load leg.xml and neck.xml (original files). -2) Reset each model to keyframe(0) if present (README-style init). -3) Export namespaced copies (leg_, neck_) WITHOUT to avoid nq mismatch when composing. - Also converts all `file="..."` paths to absolute paths, so assets still load from anywhere. -4) Compute relpose for an equality/weld constraint from the keyframe poses. -5) Generate a scene MJCF that s the two namespaced parts and adds the weld. -6) Load the scene, copy qpos/qvel/act from each part’s keyframe into the composite model, then render. - -Usage: - python scripts/render_leg_neck_connected.py \ - --leg /path/to/leg.xml \ - --neck /path/to/neck.xml \ - --out /path/to/out.gif - -Notes: -- If auto-detected root body is not what you want, pass --leg_root_body / --neck_root_body. -- Output scene + temp MJCFs are written under --workdir (default: ./_build_leg_neck). -""" - -from __future__ import annotations - -import argparse -import os -import sys -import math -import shutil -import xml.etree.ElementTree as ET -from dataclasses import dataclass -from typing import Dict, List, Tuple, Optional - -import numpy as np - -# imageio is optional unless you save gif/mp4 -try: - import imageio.v2 as imageio -except Exception: - imageio = None - -try: - import mujoco -except Exception as e: - raise SystemExit( - "Cannot import `mujoco`. Please install official MuJoCo python bindings, e.g.\n" - " pip install mujoco\n" - f"Original error: {e}" - ) - - -# ------------------------- -# XML helpers -# ------------------------- - -FILE_ATTR_WHITELIST = {"file"} # attributes that should be treated as filesystem paths - - -def _indent(elem: ET.Element, level: int = 0) -> None: - """Pretty-print indentation (in-place).""" - i = "\n" + level * " " - if len(elem): - if not elem.text or not elem.text.strip(): - elem.text = i + " " - for child in elem: - _indent(child, 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 _strip_keyframes(root: ET.Element) -> None: - """Remove blocks entirely.""" - for kf in list(root.findall("keyframe")): - root.remove(kf) - - -def _collect_rename_tokens(root: ET.Element) -> Dict[str, str]: - """ - Collect all tokens that should be namespaced: - - any 'name' - - any class-related tokens ('class', 'childclass') so defaults don't collide - """ - tokens: set[str] = set() - for el in root.iter(): - for k in ("name", "class", "childclass"): - v = el.get(k) - if v and v.strip(): - tokens.add(v.strip()) - return {t: t for t in tokens} - - -def _abspath_if_relative(path: str, base_dir: str) -> str: - # ignore already-absolute or URLs or mujoco-style placeholders - if not path: - return path - if path.startswith(("http://", "https://", "file://")): - return path - if path.startswith("${"): # environment var style - return path - if os.path.isabs(path): - return path - return os.path.abspath(os.path.join(base_dir, path)) - - -def namespace_mjcf( - in_xml: str, - out_xml: str, - prefix: str, - remove_keyframe: bool = True, - absolutize_files: bool = True, -) -> Tuple[str, Dict[str, str]]: - """ - Create a namespaced copy of MJCF: - - prefixes 'name' / 'class' / 'childclass' values - - replaces references by token matching (whitespace-separated) - - optionally removes - - optionally converts all file="..." to absolute paths based on in_xml directory - - Returns: - (root_body_name_original, rename_map_old_to_new) - """ - base_dir = os.path.dirname(os.path.abspath(in_xml)) - tree = ET.parse(in_xml) - root = tree.getroot() - - if remove_keyframe: - _strip_keyframes(root) - - # Build rename map - tokens = _collect_rename_tokens(root) - rename_map: Dict[str, str] = {old: f"{prefix}{old}" for old in tokens.keys()} - - # Apply rename on attributes - for el in root.iter(): - # absolutize file paths - if absolutize_files: - for attr, val in list(el.attrib.items()): - if attr in FILE_ATTR_WHITELIST and val: - el.set(attr, _abspath_if_relative(val, base_dir)) - - # rename primary naming attrs - for k in ("name", "class", "childclass"): - v = el.get(k) - if v and v in rename_map: - el.set(k, rename_map[v]) - - # rename references by token substitution - for attr, val in list(el.attrib.items()): - # skip file paths and root model name - if attr in FILE_ATTR_WHITELIST: - continue - if el is root and attr == "model": - continue - - if not val or not val.strip(): - continue - - parts = val.split() - changed = False - for i, tok in enumerate(parts): - if tok in rename_map: - parts[i] = rename_map[tok] - changed = True - if changed: - el.set(attr, " ".join(parts)) - - _indent(root) - os.makedirs(os.path.dirname(os.path.abspath(out_xml)), exist_ok=True) - tree.write(out_xml, encoding="utf-8", xml_declaration=True) - return out_xml, rename_map - - -def detect_root_body_name(xml_path: str) -> str: - """ - Heuristic: pick the first directly under . - """ - tree = ET.parse(xml_path) - root = tree.getroot() - worldbody = root.find("worldbody") - if worldbody is None: - raise ValueError(f"No found in: {xml_path}") - - bodies = worldbody.findall("body") - if not bodies: - raise ValueError(f"No directly under in: {xml_path}") - - # prefer named body - for b in bodies: - nm = b.get("name") - if nm and nm.strip(): - return nm.strip() - # fallback - return bodies[0].get("name", "").strip() or "unnamed_root_body" - - -# ------------------------- -# MuJoCo state helpers -# ------------------------- - -@dataclass -class ModelState: - joint_qpos: Dict[str, np.ndarray] - joint_qvel: Dict[str, np.ndarray] - actuator_act: Dict[str, np.ndarray] - body_pose_world: Dict[str, Tuple[np.ndarray, np.ndarray]] # name -> (pos(3), quat(4)) - - -def _joint_qpos_len(jnt_type: int) -> int: - # mjtJoint: FREE=0, BALL=1, SLIDE=2, HINGE=3 (MuJoCo convention) - if jnt_type == mujoco.mjtJoint.mjJNT_FREE: - return 7 - if jnt_type == mujoco.mjtJoint.mjJNT_BALL: - return 4 - return 1 - - -def _joint_dof_len(jnt_type: int) -> int: - if jnt_type == mujoco.mjtJoint.mjJNT_FREE: - return 6 - if jnt_type == mujoco.mjtJoint.mjJNT_BALL: - return 3 - return 1 - - -def load_and_reset(model_xml: str) -> Tuple["mujoco.MjModel", "mujoco.MjData"]: - m = mujoco.MjModel.from_xml_path(model_xml) - d = mujoco.MjData(m) - - if m.nkey > 0: - # README-style: reset to keyframe 0 - mujoco.mj_resetDataKeyframe(m, d, 0) - mujoco.mj_forward(m, d) - else: - mujoco.mj_resetData(m, d) - d.qpos[:] = m.qpos0 - mujoco.mj_forward(m, d) - return m, d - - -def extract_state(m: "mujoco.MjModel", d: "mujoco.MjData") -> ModelState: - joint_qpos: Dict[str, np.ndarray] = {} - joint_qvel: Dict[str, np.ndarray] = {} - - # joints - for jid in range(m.njnt): - jname = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_JOINT, jid) - if not jname: - continue - jtype = int(m.jnt_type[jid]) - qpos_adr = int(m.jnt_qposadr[jid]) - dof_adr = int(m.jnt_dofadr[jid]) - - qpos_len = _joint_qpos_len(jtype) - dof_len = _joint_dof_len(jtype) - - joint_qpos[jname] = np.array(d.qpos[qpos_adr:qpos_adr + qpos_len], dtype=float) - joint_qvel[jname] = np.array(d.qvel[dof_adr:dof_adr + dof_len], dtype=float) - - # actuator activations (if any) - actuator_act: Dict[str, np.ndarray] = {} - for aid in range(m.na): - aname = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_ACTUATOR, aid) - if not aname: - continue - actadr = int(m.actuator_actadr[aid]) - actnum = int(m.actuator_actnum[aid]) - if actnum > 0: - actuator_act[aname] = np.array(d.act[actadr:actadr + actnum], dtype=float) - - # body poses - body_pose_world: Dict[str, Tuple[np.ndarray, np.ndarray]] = {} - for bid in range(m.nbody): - bname = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, bid) - if not bname: - continue - pos = np.array(d.xpos[bid], dtype=float) - quat = np.array(d.xquat[bid], dtype=float) # w x y z - body_pose_world[bname] = (pos, quat) - - return ModelState( - joint_qpos=joint_qpos, - joint_qvel=joint_qvel, - actuator_act=actuator_act, - body_pose_world=body_pose_world, - ) - - -# ------------------------- -# Quaternion math -# ------------------------- - -def quat_mul(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: - """Hamilton product. q = [w,x,y,z].""" - w1, x1, y1, z1 = q1 - w2, x2, y2, z2 = q2 - return np.array([ - w1*w2 - x1*x2 - y1*y2 - z1*z2, - w1*x2 + x1*w2 + y1*z2 - z1*y2, - w1*y2 - x1*z2 + y1*w2 + z1*x2, - w1*z2 + x1*y2 - y1*x2 + z1*w2 - ], dtype=float) - - -def quat_conj(q: np.ndarray) -> np.ndarray: - w, x, y, z = q - return np.array([w, -x, -y, -z], dtype=float) - - -def quat_norm(q: np.ndarray) -> float: - return float(np.linalg.norm(q)) - - -def quat_inv(q: np.ndarray) -> np.ndarray: - n2 = float(np.dot(q, q)) - if n2 <= 0: - return np.array([1, 0, 0, 0], dtype=float) - return quat_conj(q) / n2 - - -def quat_to_mat(q: np.ndarray) -> np.ndarray: - """Quaternion (w,x,y,z) to rotation matrix.""" - w, x, y, z = q - # normalize - n = quat_norm(q) - if n == 0: - return np.eye(3) - w, x, y, z = (q / n).tolist() - - return np.array([ - [1 - 2*(y*y + z*z), 2*(x*y - z*w), 2*(x*z + y*w)], - [ 2*(x*y + z*w), 1 - 2*(x*x + z*z), 2*(y*z - x*w)], - [ 2*(x*z - y*w), 2*(y*z + x*w), 1 - 2*(x*x + y*y)], - ], dtype=float) - - -def compute_relpose(body1_pos, body1_quat, body2_pos, body2_quat) -> Tuple[np.ndarray, np.ndarray]: - """ - relpose is pose of body2 in body1 frame: - p_rel = R1^T (p2 - p1) - q_rel = inv(q1) * q2 - """ - R1 = quat_to_mat(body1_quat) - p_rel = R1.T @ (body2_pos - body1_pos) - q_rel = quat_mul(quat_inv(body1_quat), body2_quat) - - # normalize quaternion - n = quat_norm(q_rel) - if n > 0: - q_rel = q_rel / n - else: - q_rel = np.array([1, 0, 0, 0], dtype=float) - - return p_rel, q_rel - - -# ------------------------- -# Composite scene generation -# ------------------------- - -def write_scene_xml( - scene_xml: str, - leg_part_xml: str, - neck_part_xml: str, - leg_root_body: str, - neck_root_body: str, - relpose_pos: np.ndarray, - relpose_quat: np.ndarray, -) -> None: - """ - Create a scene MJCF which includes two parts and adds an equality/weld between their root bodies. - """ - os.makedirs(os.path.dirname(os.path.abspath(scene_xml)), exist_ok=True) - - rp = list(relpose_pos.flatten()) + list(relpose_quat.flatten()) - rp_str = " ".join(f"{x:.8g}" for x in rp) - - # Keep this scene minimal; you can add cameras/lights as needed. - mjcf = f""" - - - - - - - - - - - - - - -""" - with open(scene_xml, "w", encoding="utf-8") as f: - f.write(mjcf) - - -def apply_state_to_composite( - composite_model: "mujoco.MjModel", - composite_data: "mujoco.MjData", - src_state: ModelState, - prefix: str, -) -> None: - """ - Copy qpos/qvel/act by name from a source model state into the composite model, - assuming composite names are prefixed. - """ - # joints - for jname, qpos in src_state.joint_qpos.items(): - tgt_name = prefix + jname - jid = mujoco.mj_name2id(composite_model, mujoco.mjtObj.mjOBJ_JOINT, tgt_name) - if jid < 0: - continue - jtype = int(composite_model.jnt_type[jid]) - qpos_adr = int(composite_model.jnt_qposadr[jid]) - qpos_len = _joint_qpos_len(jtype) - if len(qpos) == qpos_len: - composite_data.qpos[qpos_adr:qpos_adr + qpos_len] = qpos - - for jname, qvel in src_state.joint_qvel.items(): - tgt_name = prefix + jname - jid = mujoco.mj_name2id(composite_model, mujoco.mjtObj.mjOBJ_JOINT, tgt_name) - if jid < 0: - continue - jtype = int(composite_model.jnt_type[jid]) - dof_adr = int(composite_model.jnt_dofadr[jid]) - dof_len = _joint_dof_len(jtype) - if len(qvel) == dof_len: - composite_data.qvel[dof_adr:dof_adr + dof_len] = qvel - - # actuator activation states - for aname, act in src_state.actuator_act.items(): - tgt_name = prefix + aname - aid = mujoco.mj_name2id(composite_model, mujoco.mjtObj.mjOBJ_ACTUATOR, tgt_name) - if aid < 0: - continue - actadr = int(composite_model.actuator_actadr[aid]) - actnum = int(composite_model.actuator_actnum[aid]) - if actnum > 0 and len(act) == actnum: - composite_data.act[actadr:actadr + actnum] = act - - -# ------------------------- -# Rendering -# ------------------------- - -def render_gif( - model: "mujoco.MjModel", - data: "mujoco.MjData", - out_path: str, - width: int = 960, - height: int = 540, - fps: int = 30, - frames: int = 180, - camera: str = "cam", - settle_steps: int = 20, -) -> None: - if imageio is None: - raise SystemExit("imageio is not installed. Please `pip install imageio` to save GIF/MP4.") - - # Let constraints settle a bit - for _ in range(settle_steps): - mujoco.mj_step(model, data) - - renderer = mujoco.Renderer(model, width=width, height=height) - imgs = [] - - for _ in range(frames): - mujoco.mj_step(model, data) - renderer.update_scene(data, camera=camera) - img = renderer.render() # HxWx3 uint8 - imgs.append(img) - - os.makedirs(os.path.dirname(os.path.abspath(out_path)), exist_ok=True) - ext = os.path.splitext(out_path)[1].lower() - - if ext == ".gif": - imageio.mimsave(out_path, imgs, fps=fps) - elif ext in (".mp4", ".mkv", ".mov"): - imageio.mimsave(out_path, imgs, fps=fps) # imageio chooses writer by extension - else: - raise ValueError(f"Unsupported output format: {ext} (use .gif or .mp4)") - - -# ------------------------- -# Main -# ------------------------- - -def main() -> int: - ap = argparse.ArgumentParser() - ap.add_argument("--leg", required=True, help="Path to leg MJCF (original).") - ap.add_argument("--neck", required=True, help="Path to neck MJCF (original).") - ap.add_argument("--out", required=True, help="Output .gif/.mp4 path.") - ap.add_argument("--workdir", default="./_build_leg_neck", help="Directory for generated temp MJCFs and scene.") - ap.add_argument("--leg_prefix", default="leg_", help="Namespace prefix for leg model.") - ap.add_argument("--neck_prefix", default="neck_", help="Namespace prefix for neck model.") - - ap.add_argument("--leg_root_body", default="", help="Override: body name in leg model to weld from.") - ap.add_argument("--neck_root_body", default="", help="Override: body name in neck model to weld to.") - - ap.add_argument("--width", type=int, default=960) - ap.add_argument("--height", type=int, default=540) - ap.add_argument("--fps", type=int, default=30) - ap.add_argument("--frames", type=int, default=180) - ap.add_argument("--settle_steps", type=int, default=20) - - args = ap.parse_args() - - leg_xml = os.path.abspath(args.leg) - neck_xml = os.path.abspath(args.neck) - workdir = os.path.abspath(args.workdir) - os.makedirs(workdir, exist_ok=True) - - # 1) Detect root bodies (in ORIGINAL names) - leg_root = args.leg_root_body.strip() or detect_root_body_name(leg_xml) - neck_root = args.neck_root_body.strip() or detect_root_body_name(neck_xml) - - # 2) Load originals and reset keyframe 0 (README-style) - leg_m, leg_d = load_and_reset(leg_xml) - neck_m, neck_d = load_and_reset(neck_xml) - leg_state = extract_state(leg_m, leg_d) - neck_state = extract_state(neck_m, neck_d) - - if leg_root not in leg_state.body_pose_world: - raise SystemExit(f"Cannot find body '{leg_root}' in leg model after reset. Try --leg_root_body.") - if neck_root not in neck_state.body_pose_world: - raise SystemExit(f"Cannot find body '{neck_root}' in neck model after reset. Try --neck_root_body.") - - leg_pos, leg_quat = leg_state.body_pose_world[leg_root] - neck_pos, neck_quat = neck_state.body_pose_world[neck_root] - - # 3) Compute relpose (neck body pose in leg body frame) from keyframe poses - rel_p, rel_q = compute_relpose(leg_pos, leg_quat, neck_pos, neck_quat) - - # 4) Write namespaced, no-keyframe copies (assets file paths become absolute) - leg_ns_xml = os.path.join(workdir, "leg_ns_nokey.xml") - neck_ns_xml = os.path.join(workdir, "neck_ns_nokey.xml") - - _, leg_map = namespace_mjcf(leg_xml, leg_ns_xml, prefix=args.leg_prefix, remove_keyframe=True, absolutize_files=True) - _, neck_map = namespace_mjcf(neck_xml, neck_ns_xml, prefix=args.neck_prefix, remove_keyframe=True, absolutize_files=True) - - leg_root_ns = args.leg_prefix + leg_root - neck_root_ns = args.neck_prefix + neck_root - - # 5) Generate composite scene XML - scene_xml = os.path.join(workdir, "leg_neck_connected_scene.xml") - write_scene_xml( - scene_xml=scene_xml, - leg_part_xml=leg_ns_xml, - neck_part_xml=neck_ns_xml, - leg_root_body=leg_root_ns, - neck_root_body=neck_root_ns, - relpose_pos=rel_p, - relpose_quat=rel_q, - ) - - # 6) Load composite and apply initial state by copying from original keyframes - comp_m = mujoco.MjModel.from_xml_path(scene_xml) - comp_d = mujoco.MjData(comp_m) - - mujoco.mj_resetData(comp_m, comp_d) - apply_state_to_composite(comp_m, comp_d, leg_state, prefix=args.leg_prefix) - apply_state_to_composite(comp_m, comp_d, neck_state, prefix=args.neck_prefix) - mujoco.mj_forward(comp_m, comp_d) - - # 7) Render - render_gif( - model=comp_m, - data=comp_d, - out_path=os.path.abspath(args.out), - width=args.width, - height=args.height, - fps=args.fps, - frames=args.frames, - camera="cam", - settle_steps=args.settle_steps, - ) - - print(f"[OK] Scene: {scene_xml}") - print(f"[OK] Output: {os.path.abspath(args.out)}") - return 0 - - -if __name__ == "__main__": - raise SystemExit(main())