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())