From 2ee15ee430c68a3fc616e476d7ae97fed445d31b Mon Sep 17 00:00:00 2001 From: qinyiquan57-debug Date: Sun, 21 Dec 2025 12:47:15 +0800 Subject: [PATCH 1/4] =?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/4] =?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/4] =?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/4] =?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())