cozyclay 1.0.0

package/tools/ardy/dump-npz.py

@@ -0,0 +1,205 @@
+#!/usr/bin/env python3
+"""Dump one frame of an ARDY motion npz as a JSON document on stdout.
+
+Runs under the ARDY box python (``~/ardy/.venv/bin/python``); it needs numpy
+but no torch. Usage::
+
+    python dump-npz.py <motion.npz> <frame>
+
+Emitted document (schema "ardy.frame.v1"):
+
+    {
+      "schema": "ardy.frame.v1",
+      "source_npz": "<basename of the npz>",
+      "frame": <requested frame index>,
+      "frames_total": <frame count of the motion>,
+      "members": {"<member name>": {"shape": [...], "dtype": "..."}, ...},
+      "local_rot_mats": [ 27 x [[3],[3],[3]] row-major ],
+      "posed_joints":   [ 27 x [x, y, z] ]
+    }
+
+Member names and axis order match the ARDY archives exactly as consumed by
+CozyClay (blender-addon/cclay): ``local_rot_mats[frame, joint]`` is the
+per-joint LOCAL rotation matrix in cskel27 order (Hips = joint 0), and
+``posed_joints[frame, joint]`` the joint center position. ``foot_contacts``
+is optional and reported only when present. ``members`` lists every array in
+the archive (sorted), including carried members like ``fps``, so a consumer
+can see the exact contents without re-opening the npz.
+
+Before emitting, every one of the 27 rotation matrices of the requested frame
+is checked for finite entries, orthonormal rows/columns, and determinant +1,
+with the same tolerance CozyClay's motion_retarget uses
+(ROTATION_MATRIX_TOLERANCE = 1e-3 on squared norms, pairwise dots, and
+determinant). On the first violation the matrix index and the failing
+quantity are printed to stderr, the exit status is 1, and NO JSON is emitted:
+a garbage rotation must not silently reach a downstream consumer.
+"""
+
+import json
+import math
+import sys
+
+import numpy as np
+
+SCHEMA = "ardy.frame.v1"
+JOINTS = 27
+# Mirror of CozyClay motion_retarget.ROTATION_MATRIX_TOLERANCE: applies to
+# squared row/column norms, pairwise dot products, and determinant, in
+# float64. float32 ARDY serialization noise stays far below 1e-3.
+TOLERANCE = 1e-3
+
+
+def _fail(message):
+    """Report a fatal error on stderr and exit 1 without emitting JSON."""
+    print(f"dump-npz: {message}", file=sys.stderr)
+    sys.exit(1)
+
+
+def _check_rotation(matrix, frame, joint):
+    """Return a list of violation strings for one 3x3 matrix, or [] if clean."""
+    violations = []
+    rows = [[float(matrix[i][j]) for j in range(3)] for i in range(3)]
+    columns = [[rows[i][j] for i in range(3)] for j in range(3)]
+    prefix = f"frame {frame} joint {joint}"
+    for label, vectors in (("row", rows), ("column", columns)):
+        for index, vector in enumerate(vectors):
+            squared_norm = sum(c * c for c in vector)
+            if not math.isfinite(squared_norm):
+                violations.append(
+                    f"{prefix} {label} {index} has non-finite entries"
+                )
+            elif abs(squared_norm - 1.0) > TOLERANCE:
+                violations.append(
+                    f"{prefix} {label} {index} squared norm {squared_norm:.6f} "
+                    f"differs from 1.0 by more than {TOLERANCE}"
+                )
+        for first, second in ((0, 1), (0, 2), (1, 2)):
+            dot = sum(
+                vectors[first][i] * vectors[second][i] for i in range(3)
+            )
+            if not math.isfinite(dot):
+                violations.append(
+                    f"{prefix} {label} {first}/{second} dot is non-finite"
+                )
+            elif abs(dot) > TOLERANCE:
+                violations.append(
+                    f"{prefix} {label} {first}/{second} dot {dot:.6f} "
+                    f"exceeds {TOLERANCE} in magnitude"
+                )
+    determinant = (
+        rows[0][0] * (rows[1][1] * rows[2][2] - rows[1][2] * rows[2][1])
+        - rows[0][1] * (rows[1][0] * rows[2][2] - rows[1][2] * rows[2][0])
+        + rows[0][2] * (rows[1][0] * rows[2][1] - rows[1][1] * rows[2][0])
+    )
+    if not math.isfinite(determinant):
+        violations.append(f"{prefix} determinant is non-finite")
+    elif abs(determinant - 1.0) > TOLERANCE:
+        violations.append(
+            f"{prefix} determinant {determinant:.6f} differs from 1.0 "
+            f"by more than {TOLERANCE}"
+        )
+    return violations
+
+
+def main(argv):
+    if len(argv) != 3:
+        _fail(f"usage: {argv[0]} <motion.npz> <frame>")
+    npz_path, frame_text = argv[1], argv[2]
+    try:
+        frame = int(frame_text)
+    except ValueError:
+        _fail(f"frame must be an integer, got {frame_text!r}")
+    if frame < 0:
+        _fail(f"frame must be >= 0, got {frame}")
+
+    try:
+        data = np.load(npz_path, allow_pickle=False)
+    except Exception as exc:
+        _fail(f"cannot load {npz_path}: {exc}")
+
+    try:
+        if "local_rot_mats" not in data.files:
+            _fail(f"{npz_path} has no 'local_rot_mats' member")
+        if "posed_joints" not in data.files:
+            _fail(f"{npz_path} has no 'posed_joints' member")
+        rotations = data["local_rot_mats"]
+        joints = data["posed_joints"]
+        if rotations.shape[1:] != (JOINTS, 3, 3):
+            _fail(
+                f"local_rot_mats must have shape (F, {JOINTS}, 3, 3), "
+                f"got {rotations.shape}"
+            )
+        if joints.shape[1:] != (JOINTS, 3):
+            _fail(
+                f"posed_joints must have shape (F, {JOINTS}, 3), "
+                f"got {joints.shape}"
+            )
+        if joints.shape[0] != rotations.shape[0]:
+            _fail(
+                "local_rot_mats and posed_joints frame counts differ "
+                f"({rotations.shape[0]} vs {joints.shape[0]})"
+            )
+        frames_total = int(rotations.shape[0])
+        if frames_total < 1:
+            _fail(f"motion has no frames ({frames_total})")
+        if frame >= frames_total:
+            _fail(
+                f"frame {frame} out of range: motion has {frames_total} frames"
+            )
+
+        # Validate every rotation matrix of the requested frame BEFORE any
+        # JSON is written; a violation aborts with a stderr report and a
+        # non-zero exit status instead of emitting garbage.
+        violations = []
+        for joint in range(JOINTS):
+            violations.extend(_check_rotation(rotations[frame][joint], frame, joint))
+        for joint in range(JOINTS):
+            for axis in range(3):
+                if not math.isfinite(float(joints[frame][joint][axis])):
+                    violations.append(
+                        f"frame {frame} joint {joint} axis {axis} "
+                        "position is not finite"
+                    )
+        if violations:
+            for violation in violations[:20]:
+                print(f"dump-npz: VIOLATION {violation}", file=sys.stderr)
+            if len(violations) > 20:
+                print(
+                    f"dump-npz: ... and {len(violations) - 20} more",
+                    file=sys.stderr,
+                )
+            _fail(
+                f"refusing to emit {npz_path} frame {frame}: "
+                f"{len(violations)} rotation/position check(s) failed"
+            )
+
+        members = {}
+        for name in sorted(data.files):
+            member = data[name]
+            members[name] = {
+                "shape": [int(size) for size in member.shape],
+                "dtype": str(member.dtype),
+            }
+
+        document = {
+            "schema": SCHEMA,
+            "source_npz": npz_path.rsplit("/", 1)[-1],
+            "frame": frame,
+            "frames_total": frames_total,
+            "members": members,
+            "local_rot_mats": [
+                [[float(value) for value in row] for row in rotations[frame][joint]]
+                for joint in range(JOINTS)
+            ],
+            "posed_joints": [
+                [float(value) for value in joints[frame][joint]]
+                for joint in range(JOINTS)
+            ],
+        }
+        print(json.dumps(document, indent=2))
+    finally:
+        data.close()
+
+
+if __name__ == "__main__":
+    main(sys.argv)

package/tools/ardy/extract-rest.mjs

@@ -0,0 +1,299 @@
+#!/usr/bin/env node
+/**
+ * Extract the cskel27 rest rotations (Rb) from a CozyClay T-pose FBX rig.
+ *
+ * Rb is the rotation part of each bone's rest (bind) transform in the
+ * armature root's space. The ARDY retarget feeds it into
+ * `basis = Rb^T @ L @ Rb` (CozyClay motion_retarget.py), where it is the
+ * Blender-side `bone.matrix_local.to_3x3()` -- the bone's rest orientation in
+ * armature space, NOT its parent-relative rotation and NOT its world
+ * orientation including the FBX object transform. Bind data comes from the
+ * skeleton's boneInverses (three.js FBXLoader builds those from the FBX
+ * TransformLink matrices, the authoritative bind), with the armature
+ * container's own world transform divided out.
+ *
+ * Usage: node tools/ardy/extract-rest.mjs
+ * Writes: public/ardy/cskel27-rest.json (x-bot rig), 27 entries in cskel27
+ * index order. Also extracts y-bot-tpose.fbx and prints the maximum
+ * per-joint angular difference between the two rigs' rest rotations.
+ *
+ * Exits non-zero if any emitted matrix fails the orthonormality + det checks
+ * (same tolerances as motion_retarget._validate_rotation_matrix).
+ */
+import * as THREE from "three";
+import { FBXLoader } from "three/examples/jsm/loaders/FBXLoader.js";
+import { mkdirSync, readFileSync, writeFileSync } from "node:fs";
+import { dirname, resolve } from "node:path";
+import { fileURLToPath } from "node:url";
+// Project matcher primitive: strip every non-alphanumeric char, lowercase.
+// Reused here so the FBX side and the pose studio can never drift apart.
+import { normalizeBoneName } from "../../src/poses.js";
+
+const HERE = dirname(fileURLToPath(import.meta.url));
+const MODEL_DIR = resolve(HERE, "../../public/models");
+const OUT_FILE = resolve(HERE, "../../public/ardy/cskel27-rest.json");
+
+/** cskel27 joint order; array index == npz joint index. Verbatim from
+ * motion_retarget.CSKEL27_JOINTS. */
+const CSKEL27_JOINTS = [
+	"Hips", "Spine", "Spine1", "Spine2", "Spine3", "Neck", "Head",
+	"RightShoulder", "RightArm", "RightForeArm", "RightHand",
+	"RightHandEnd", "RightHandThumb1",
+	"LeftShoulder", "LeftArm", "LeftForeArm", "LeftHand",
+	"LeftHandEnd", "LeftHandThumb1",
+	"RightUpLeg", "RightLeg", "RightFoot", "RightToeBase",
+	"LeftUpLeg", "LeftLeg", "LeftFoot", "LeftToeBase",
+];
+
+/** Mirrors ROTATION_MATRIX_TOLERANCE in CozyClay motion_retarget.py. */
+const ROTATION_MATRIX_TOLERANCE = 1e-3;
+
+// FBXLoader touches two browser globals on paths this script never exercises
+// (embedded-texture blob URLs, camera aspect fallback). Keep the loader
+// loadable under Node without pretending those paths work. No fetch/self shim
+// is needed: parse() reads the buffer directly and FBXLoader never references
+// `self`.
+if (typeof globalThis.window === "undefined") {
+	globalThis.window = {
+		innerWidth: 1920,
+		innerHeight: 1080,
+		URL: {
+			createObjectURL(blob) {
+				if (typeof URL.createObjectURL === "function") {
+					return URL.createObjectURL(blob);
+				}
+				throw new Error(
+					"extract-rest: embedded FBX texture loading is not supported under Node"
+				);
+			},
+		},
+	};
+}
+
+/** The project's bone-matching rule (poses.js boneMatches): normalised names
+ * equal, or one is a suffix of the other (`mixamorigleftarm` vs `leftarm`). */
+function matchesJoint(boneName, cskelName) {
+	const norm = normalizeBoneName(boneName);
+	const target = normalizeBoneName(cskelName);
+	return norm === target || norm.endsWith(target) || target.endsWith(norm);
+}
+
+/** Find the control skeleton of a parsed FBX root: the one whose root bone
+ * hangs off the armature container (a non-bone). Mixamo exports also nest an
+ * identity "skinned" copy of every bone under its control bone; those copy
+ * skeletons fail this test (their root's parent is a bone), so this rule
+ * picks the real hierarchy skeleton regardless of mesh order. */
+function findControlSkeleton(root) {
+	const candidates = [];
+	root.traverse((object) => {
+		if (!object.isSkinnedMesh || !object.skeleton) return;
+		const skeleton = object.skeleton;
+		if (!skeleton.bones.length) return;
+		const rootBone = skeleton.bones.find(
+			(bone) => !bone.parent || !bone.parent.isBone
+		);
+		if (rootBone) candidates.push({ skeleton, rootBone });
+	});
+	if (!candidates.length) {
+		throw new Error("no skinned skeleton with a non-bone root found");
+	}
+	if (candidates.length > 1) {
+		console.warn(
+			`extract-rest: ${candidates.length} control skeletons; using the first`
+		);
+	}
+	return candidates[0];
+}
+
+/** Load a rig and index every bind bone by normalised name.
+ * Returns Map<normalised name, { name, bind }> where `bind` is the bone's
+ * rest (bind) transform in armature space as a Matrix4. */
+function loadRig(rigName) {
+	const path = resolve(MODEL_DIR, `${rigName}.fbx`);
+	const buffer = readFileSync(path);
+	const arrayBuffer = buffer.buffer.slice(
+		buffer.byteOffset,
+		buffer.byteOffset + buffer.byteLength
+	);
+	const root = new FBXLoader().parse(arrayBuffer, path);
+	root.updateMatrixWorld(true);
+
+	const { skeleton, rootBone } = findControlSkeleton(root);
+	// Blender's matrix_local divides out the armature object's world
+	// transform; the FBX analogue is the skeleton root's non-bone parent.
+	const armatureWorld = rootBone.parent
+		? rootBone.parent.matrixWorld
+		: new THREE.Matrix4();
+	const toArmature = armatureWorld.clone().invert();
+
+	const bindByName = new Map();
+	skeleton.bones.forEach((bone, index) => {
+		const bindGlobal = skeleton.boneInverses[index].clone().invert();
+		// Keep the full Matrix4: Quaternion.setFromRotationMatrix reads 4x4
+		// element indices, so a Matrix3 would silently produce NaN.
+		const bind = toArmature.clone().multiply(bindGlobal);
+		bindByName.set(normalizeBoneName(bone.name), { name: bone.name, bind });
+	});
+	return bindByName;
+}
+
+/** Column-major Matrix3 -> row-major 3x3 arrays. The JSON schema is
+ * row-major, matching motion_retarget's nested-list matrices. */
+function rowMajor(m3) {
+	const e = m3.elements;
+	return [
+		[e[0], e[3], e[6]],
+		[e[1], e[4], e[7]],
+		[e[2], e[5], e[8]],
+	];
+}
+
+/** Verify an orthonormal proper rotation exactly like
+ * motion_retarget._validate_rotation_matrix: every row and column must have
+ * squared norm 1 and pairwise dot 0 within the tolerance, and the determinant
+ * must be +1 within the tolerance. Returns null when valid, else a
+ * description of the failure. */
+function validateRotationMatrix(rows) {
+	const tolerance = ROTATION_MATRIX_TOLERANCE;
+	const columns = [
+		[rows[0][0], rows[1][0], rows[2][0]],
+		[rows[0][1], rows[1][1], rows[2][1]],
+		[rows[0][2], rows[1][2], rows[2][2]],
+	];
+	for (const vectors of [rows, columns]) {
+		for (const vector of vectors) {
+			const squaredNorm =
+				vector[0] * vector[0] + vector[1] * vector[1] + vector[2] * vector[2];
+			if (
+				!Number.isFinite(squaredNorm) ||
+				Math.abs(squaredNorm - 1) > tolerance
+			) {
+				return `squared norm ${squaredNorm}`;
+			}
+		}
+		for (const [first, second] of [[0, 1], [0, 2], [1, 2]]) {
+			const dot =
+				vectors[first][0] * vectors[second][0] +
+				vectors[first][1] * vectors[second][1] +
+				vectors[first][2] * vectors[second][2];
+			if (!Number.isFinite(dot) || Math.abs(dot) > tolerance) {
+				return `pairwise dot ${dot}`;
+			}
+		}
+	}
+	const determinant =
+		rows[0][0] * (rows[1][1] * rows[2][2] - rows[1][2] * rows[2][1]) -
+		rows[0][1] * (rows[1][0] * rows[2][2] - rows[1][2] * rows[2][0]) +
+		rows[0][2] * (rows[1][0] * rows[2][1] - rows[1][1] * rows[2][0]);
+	if (!Number.isFinite(determinant) || Math.abs(determinant - 1) > tolerance) {
+		return `determinant ${determinant}`;
+	}
+	return null;
+}
+
+/** Orthonormalise a near-rotation matrix and emit it. The quaternion
+ * round-trip yields the closest proper rotation, so the emitted matrix always
+ * satisfies the rotation checks; the RAW bind matrix is validated first,
+ * which is the check that can actually fail (scale/shear/reflection would
+ * survive the tolerance and must not be shipped as a rest rotation). */
+function restRotationRows(bind, jointName) {
+	const rawFailure = validateRotationMatrix(rowMajor(new THREE.Matrix3().setFromMatrix4(bind)));
+	if (rawFailure !== null) {
+		throw new Error(
+			`${jointName}: raw bind matrix is not a proper rotation (${rawFailure})`
+		);
+	}
+	const closest = new THREE.Matrix4()
+		.makeRotationFromQuaternion(new THREE.Quaternion().setFromRotationMatrix(bind));
+	const rows = rowMajor(new THREE.Matrix3().setFromMatrix4(closest));
+	const emittedFailure = validateRotationMatrix(rows);
+	if (emittedFailure !== null) {
+		throw new Error(
+			`${jointName}: orthonormalised rest rotation failed validation (${emittedFailure})`
+		);
+	}
+	return rows;
+}
+
+/** Angle in degrees between two rotations (the rotation of q1^-1 @ q2). */
+function angleDegrees(q1, q2) {
+	const relative = q1.clone().invert().multiply(q2);
+	const axisLength = Math.hypot(relative.x, relative.y, relative.z);
+	return (2 * Math.atan2(axisLength, relative.w) * 180) / Math.PI;
+}
+
+function main() {
+	const xRig = loadRig("x-bot-tpose");
+
+	const joints = [];
+	const missing = [];
+	for (let index = 0; index < CSKEL27_JOINTS.length; index++) {
+		const name = CSKEL27_JOINTS[index];
+		let matchedBone = null;
+		let rest = null;
+		for (const entry of xRig.values()) {
+			if (matchesJoint(entry.name, name)) {
+				matchedBone = entry.name;
+				rest = restRotationRows(entry.bind, name);
+				break;
+			}
+		}
+		if (matchedBone === null) missing.push(name);
+		joints.push({ name, index, matched_bone: matchedBone, rest });
+	}
+
+	const json = {
+		schema: "cskel27.rest.v1",
+		rig: "x-bot-tpose",
+		joints,
+		missing,
+	};
+	mkdirSync(dirname(OUT_FILE), { recursive: true });
+	writeFileSync(OUT_FILE, `${JSON.stringify(json, null, 2)}\n`);
+
+	const resolved = joints.filter((joint) => joint.rest !== null).length;
+	console.log(`wrote ${OUT_FILE}`);
+	console.log(`resolved ${resolved}/${CSKEL27_JOINTS.length} cskel27 joints`);
+	for (const joint of joints) {
+		const match = joint.matched_bone ?? "MISSING";
+		console.log(`  ${joint.index.toString().padStart(2)} ${joint.name.padEnd(15)} -> ${match}`);
+	}
+	console.log(`missing: ${missing.length ? missing.join(", ") : "none"}`);
+
+	// Same extraction against the y-bot rig; the gap between the two rigs'
+	// rest rotations is why poses travel as deltas, not absolute rotations.
+	const yRig = loadRig("y-bot-tpose");
+	let maxAngle = -1;
+	let maxJoint = null;
+	let compared = 0;
+	for (const joint of joints) {
+		if (joint.rest === null) continue;
+		const yEntry = yRig.get(normalizeBoneName(joint.matched_bone));
+		if (!yEntry) continue;
+		compared += 1;
+		const angle = angleDegrees(
+			new THREE.Quaternion().setFromRotationMatrix(
+				new THREE.Matrix4().setFromMatrix3(matrixFromRows(joint.rest))
+			),
+			new THREE.Quaternion().setFromRotationMatrix(yEntry.bind)
+		);
+		if (angle > maxAngle) {
+			maxAngle = angle;
+			maxJoint = joint.name;
+		}
+	}
+	console.log(
+		`max per-joint rest rotation difference x-bot vs y-bot: ${maxAngle.toFixed(3)} deg (${maxJoint}, ${compared} joints compared)`
+	);
+}
+
+/** Row-major nested arrays -> Matrix3. Matrix3.set takes row-major arguments. */
+function matrixFromRows(rows) {
+	return new THREE.Matrix3().set(
+		rows[0][0], rows[0][1], rows[0][2],
+		rows[1][0], rows[1][1], rows[1][2],
+		rows[2][0], rows[2][1], rows[2][2]
+	);
+}
+
+main();